From 3b3a1245d9a4fd50f663e144a5909000db84a2c3 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Tue, 7 Apr 2020 15:48:27 +0200 Subject: [PATCH 01/16] fix shared on windows --- CMakeLists.txt | 8 +- include/fcl/CMakeLists.txt | 21 + include/fcl/broadphase/broadphase_SSaP-inl.h | 6 +- include/fcl/broadphase/broadphase_SSaP.h | 12 +- include/fcl/broadphase/broadphase_SaP-inl.h | 4 +- include/fcl/broadphase/broadphase_SaP.h | 16 +- .../broadphase/broadphase_bruteforce-inl.h | 4 +- .../fcl/broadphase/broadphase_bruteforce.h | 6 +- .../broadphase_collision_manager-inl.h | 2 +- .../broadphase/broadphase_collision_manager.h | 6 +- ...adphase_continuous_collision_manager-inl.h | 2 +- .../broadphase_continuous_collision_manager.h | 6 +- .../broadphase_dynamic_AABB_tree-inl.h | 36 +- .../broadphase/broadphase_dynamic_AABB_tree.h | 10 +- .../broadphase_dynamic_AABB_tree_array-inl.h | 36 +- .../broadphase_dynamic_AABB_tree_array.h | 12 +- .../broadphase/broadphase_interval_tree-inl.h | 4 +- .../fcl/broadphase/broadphase_interval_tree.h | 10 +- .../broadphase/broadphase_spatialhash-inl.h | 4 +- .../fcl/broadphase/broadphase_spatialhash.h | 4 +- .../fcl/broadphase/detail/hierarchy_tree.h | 52 +- .../broadphase/detail/hierarchy_tree_array.h | 46 +- .../fcl/broadphase/detail/interval_tree-inl.h | 2 +- include/fcl/broadphase/detail/interval_tree.h | 8 +- .../detail/interval_tree_node-inl.h | 2 +- .../broadphase/detail/interval_tree_node.h | 8 +- include/fcl/broadphase/detail/morton-inl.h | 5 +- include/fcl/broadphase/detail/morton.h | 13 +- .../fcl/broadphase/detail/simple_hash_table.h | 2 +- .../broadphase/detail/simple_interval-inl.h | 2 +- .../fcl/broadphase/detail/simple_interval.h | 4 +- .../fcl/broadphase/detail/sparse_hash_table.h | 8 +- .../fcl/broadphase/detail/spatial_hash-inl.h | 4 +- include/fcl/broadphase/detail/spatial_hash.h | 6 +- include/fcl/common/detail/profiler.h | 10 +- include/fcl/common/exception.h | 2 +- include/fcl/common/time.h | 6 +- include/fcl/common/types.h | 2 +- include/fcl/geometry/bvh/BVH_model.h | 8 +- include/fcl/geometry/bvh/BVH_utility-inl.h | 5 +- include/fcl/geometry/bvh/BVH_utility.h | 3 - include/fcl/geometry/bvh/BV_node.h | 2 +- include/fcl/geometry/bvh/BV_node_base.h | 4 +- include/fcl/geometry/bvh/detail/BVH_front.h | 4 +- include/fcl/geometry/bvh/detail/BV_fitter.h | 2 +- .../fcl/geometry/bvh/detail/BV_fitter_base.h | 2 +- include/fcl/geometry/bvh/detail/BV_splitter.h | 2 +- .../geometry/bvh/detail/BV_splitter_base.h | 2 +- include/fcl/geometry/collision_geometry-inl.h | 2 +- include/fcl/geometry/collision_geometry.h | 2 +- include/fcl/geometry/octree/octree-inl.h | 3 +- include/fcl/geometry/octree/octree.h | 5 +- include/fcl/geometry/shape/box-inl.h | 2 +- include/fcl/geometry/shape/box.h | 2 +- include/fcl/geometry/shape/capsule-inl.h | 2 +- include/fcl/geometry/shape/capsule.h | 10 +- include/fcl/geometry/shape/cone-inl.h | 2 +- include/fcl/geometry/shape/cone.h | 10 +- include/fcl/geometry/shape/convex-inl.h | 2 +- include/fcl/geometry/shape/convex.h | 4 +- include/fcl/geometry/shape/cylinder-inl.h | 2 +- include/fcl/geometry/shape/cylinder.h | 14 +- include/fcl/geometry/shape/ellipsoid-inl.h | 2 +- include/fcl/geometry/shape/ellipsoid.h | 2 +- include/fcl/geometry/shape/halfspace-inl.h | 3 +- include/fcl/geometry/shape/halfspace.h | 7 +- include/fcl/geometry/shape/plane-inl.h | 3 +- include/fcl/geometry/shape/plane.h | 21 +- include/fcl/geometry/shape/shape_base-inl.h | 2 +- include/fcl/geometry/shape/shape_base.h | 2 +- include/fcl/geometry/shape/sphere-inl.h | 2 +- include/fcl/geometry/shape/sphere.h | 2 +- include/fcl/geometry/shape/triangle_p-inl.h | 2 +- include/fcl/geometry/shape/triangle_p.h | 4 +- include/fcl/geometry/shape/utility-inl.h | 138 +- include/fcl/geometry/shape/utility.h | 17 - include/fcl/math/bv/AABB-inl.h | 2 +- include/fcl/math/bv/AABB.h | 2 +- include/fcl/math/bv/OBB-inl.h | 7 +- include/fcl/math/bv/OBB.h | 15 +- include/fcl/math/bv/OBBRSS-inl.h | 3 +- include/fcl/math/bv/OBBRSS.h | 7 +- include/fcl/math/bv/RSS-inl.h | 8 +- include/fcl/math/bv/RSS.h | 10 +- include/fcl/math/bv/kDOP-inl.h | 38 +- include/fcl/math/bv/kDOP.h | 10 +- include/fcl/math/bv/kIOS-inl.h | 2 +- include/fcl/math/bv/kIOS.h | 9 +- include/fcl/math/bv/utility-inl.h | 107 +- include/fcl/math/bv/utility.h | 2 - include/fcl/math/constants.h | 2 +- include/fcl/math/detail/polysolver-inl.h | 402 +-- include/fcl/math/detail/polysolver.h | 160 +- include/fcl/math/detail/project-inl.h | 640 ++--- include/fcl/math/detail/project.h | 192 +- include/fcl/math/detail/seed.h | 2 +- include/fcl/math/geometry-inl.h | 41 +- include/fcl/math/geometry.h | 20 - .../fcl/math/motion/bv_motion_bound_visitor.h | 2 +- include/fcl/math/motion/interp_motion-inl.h | 2 +- include/fcl/math/motion/interp_motion.h | 8 +- include/fcl/math/motion/motion_base-inl.h | 2 +- include/fcl/math/motion/screw_motion-inl.h | 2 +- include/fcl/math/motion/screw_motion.h | 2 +- include/fcl/math/motion/spline_motion-inl.h | 2 +- include/fcl/math/motion/spline_motion.h | 8 +- .../math/motion/taylor_model/interval-inl.h | 4 +- .../fcl/math/motion/taylor_model/interval.h | 4 +- .../motion/taylor_model/interval_matrix-inl.h | 3 +- .../motion/taylor_model/interval_matrix.h | 3 +- .../motion/taylor_model/interval_vector-inl.h | 4 +- .../motion/taylor_model/interval_vector.h | 8 +- .../motion/taylor_model/taylor_matrix-inl.h | 9 +- .../math/motion/taylor_model/taylor_matrix.h | 11 +- .../motion/taylor_model/taylor_model-inl.h | 8 +- .../math/motion/taylor_model/taylor_model.h | 12 +- .../motion/taylor_model/taylor_vector-inl.h | 6 +- .../math/motion/taylor_model/taylor_vector.h | 10 +- .../motion/taylor_model/time_interval-inl.h | 2 +- .../math/motion/taylor_model/time_interval.h | 2 +- .../math/motion/tbv_motion_bound_visitor.h | 2 +- .../fcl/math/motion/translation_motion-inl.h | 2 +- include/fcl/math/motion/translation_motion.h | 2 +- .../triangle_motion_bound_visitor-inl.h | 2 +- .../motion/triangle_motion_bound_visitor.h | 2 +- include/fcl/math/rng-inl.h | 2 +- include/fcl/math/rng.h | 2 +- include/fcl/math/sampler/sampler_base.h | 4 +- include/fcl/math/sampler/sampler_r.h | 2 +- include/fcl/math/sampler/sampler_se2-inl.h | 2 +- include/fcl/math/sampler/sampler_se2.h | 2 +- .../fcl/math/sampler/sampler_se2_disk-inl.h | 2 +- include/fcl/math/sampler/sampler_se2_disk.h | 2 +- .../fcl/math/sampler/sampler_se3_euler-inl.h | 2 +- include/fcl/math/sampler/sampler_se3_euler.h | 2 +- .../math/sampler/sampler_se3_euler_ball-inl.h | 2 +- .../fcl/math/sampler/sampler_se3_euler_ball.h | 4 +- .../fcl/math/sampler/sampler_se3_quat-inl.h | 2 +- include/fcl/math/sampler/sampler_se3_quat.h | 2 +- .../math/sampler/sampler_se3_quat_ball-inl.h | 2 +- .../fcl/math/sampler/sampler_se3_quat_ball.h | 2 +- include/fcl/math/triangle.h | 2 +- include/fcl/math/variance3-inl.h | 2 +- include/fcl/math/variance3.h | 2 +- include/fcl/narrowphase/collision-inl.h | 8 +- include/fcl/narrowphase/collision.h | 2 - .../fcl/narrowphase/collision_object-inl.h | 2 +- include/fcl/narrowphase/collision_object.h | 2 +- .../fcl/narrowphase/collision_request-inl.h | 2 +- include/fcl/narrowphase/collision_request.h | 4 +- .../fcl/narrowphase/collision_result-inl.h | 2 +- include/fcl/narrowphase/collision_result.h | 4 +- include/fcl/narrowphase/contact-inl.h | 2 +- include/fcl/narrowphase/contact.h | 6 +- include/fcl/narrowphase/contact_point-inl.h | 4 +- include/fcl/narrowphase/contact_point.h | 4 +- .../narrowphase/continuous_collision-inl.h | 14 +- .../fcl/narrowphase/continuous_collision.h | 4 - .../continuous_collision_object-inl.h | 2 +- .../narrowphase/continuous_collision_object.h | 2 +- .../continuous_collision_request-inl.h | 2 +- .../continuous_collision_request.h | 6 +- .../continuous_collision_result-inl.h | 2 +- .../narrowphase/continuous_collision_result.h | 6 +- include/fcl/narrowphase/cost_source-inl.h | 2 +- include/fcl/narrowphase/cost_source.h | 2 +- .../convexity_based_algorithm/epa-inl.h | 2 +- .../detail/convexity_based_algorithm/epa.h | 8 +- .../convexity_based_algorithm/gjk-inl.h | 2 +- .../detail/convexity_based_algorithm/gjk.h | 6 +- .../gjk_libccd-inl.h | 19 +- .../convexity_based_algorithm/gjk_libccd.h | 27 +- .../minkowski_diff-inl.h | 3 +- .../minkowski_diff.h | 6 +- .../detail/failed_at_this_configuration.h | 6 +- .../narrowphase/detail/gjk_solver_indep-inl.h | 2 +- .../fcl/narrowphase/detail/gjk_solver_indep.h | 2 +- .../detail/gjk_solver_libccd-inl.h | 2 +- .../narrowphase/detail/gjk_solver_libccd.h | 2 +- .../primitive_shape_algorithm/box_box-inl.h | 5 + .../primitive_shape_algorithm/box_box.h | 6 - .../capsule_capsule-inl.h | 16 +- .../capsule_capsule.h | 12 +- .../primitive_shape_algorithm/halfspace-inl.h | 12 + .../primitive_shape_algorithm/halfspace.h | 17 +- .../primitive_shape_algorithm/plane-inl.h | 11 + .../detail/primitive_shape_algorithm/plane.h | 16 +- .../sphere_box-inl.h | 36 +- .../primitive_shape_algorithm/sphere_box.h | 16 +- .../sphere_capsule-inl.h | 3 + .../sphere_capsule.h | 3 - .../sphere_cylinder-inl.h | 40 +- .../sphere_cylinder.h | 20 +- .../sphere_sphere-inl.h | 6 +- .../sphere_triangle-inl.h | 6 + .../sphere_triangle.h | 6 - .../triangle_distance-inl.h | 934 +++---- .../triangle_distance.h | 228 +- .../collision/bvh_collision_traversal_node.h | 4 +- .../bvh_shape_collision_traversal_node.h | 2 +- .../collision_traversal_node_base-inl.h | 2 +- .../collision/collision_traversal_node_base.h | 4 +- .../traversal/collision/intersect-inl.h | 2292 ++++++++--------- .../detail/traversal/collision/intersect.h | 530 ++-- .../mesh_collision_traversal_node-inl.h | 12 +- .../collision/mesh_collision_traversal_node.h | 21 +- ..._continuous_collision_traversal_node-inl.h | 2 +- ...mesh_continuous_collision_traversal_node.h | 5 +- .../mesh_shape_collision_traversal_node.h | 18 +- .../shape_bvh_collision_traversal_node.h | 2 +- .../shape_collision_traversal_node.h | 2 +- .../shape_mesh_collision_traversal_node-inl.h | 20 - .../shape_mesh_collision_traversal_node.h | 15 +- .../detail/traversal/collision_node-inl.h | 5 + .../detail/traversal/collision_node.h | 5 - .../distance/bvh_distance_traversal_node.h | 2 +- .../bvh_shape_distance_traversal_node.h | 2 +- .../conservative_advancement_stack_data-inl.h | 2 +- .../conservative_advancement_stack_data.h | 2 +- .../distance_traversal_node_base-inl.h | 2 +- .../distance/distance_traversal_node_base.h | 2 +- ...servative_advancement_traversal_node-inl.h | 6 +- ..._conservative_advancement_traversal_node.h | 17 +- .../mesh_distance_traversal_node-inl.h | 9 +- .../distance/mesh_distance_traversal_node.h | 19 +- ..._conservative_advancement_traversal_node.h | 8 +- .../mesh_shape_distance_traversal_node.h | 14 +- .../shape_bvh_distance_traversal_node.h | 4 +- ..._conservative_advancement_traversal_node.h | 2 +- .../distance/shape_distance_traversal_node.h | 2 +- ..._conservative_advancement_traversal_node.h | 8 +- .../shape_mesh_distance_traversal_node-inl.h | 4 - .../shape_mesh_distance_traversal_node.h | 16 +- .../mesh_octree_collision_traversal_node.h | 2 +- .../octree_collision_traversal_node.h | 2 +- .../octree_mesh_collision_traversal_node.h | 2 +- .../octree_shape_collision_traversal_node.h | 2 +- .../shape_octree_collision_traversal_node.h | 2 +- .../mesh_octree_distance_traversal_node.h | 2 +- .../distance/octree_distance_traversal_node.h | 2 +- .../octree_mesh_distance_traversal_node.h | 2 +- .../octree_shape_distance_traversal_node.h | 2 +- .../shape_octree_distance_traversal_node.h | 2 +- .../detail/traversal/octree/octree_solver.h | 2 +- .../traversal/traversal_node_base-inl.h | 2 +- .../detail/traversal/traversal_node_base.h | 6 +- .../detail/traversal/traversal_recurse-inl.h | 20 +- .../detail/traversal/traversal_recurse.h | 7 - include/fcl/narrowphase/distance-inl.h | 2 + include/fcl/narrowphase/distance.h | 4 +- .../fcl/narrowphase/distance_request-inl.h | 2 +- include/fcl/narrowphase/distance_request.h | 2 +- include/fcl/narrowphase/distance_result-inl.h | 7 +- include/fcl/narrowphase/distance_result.h | 4 +- src/broadphase/broadphase_SSaP.cpp | 4 +- src/broadphase/broadphase_SaP.cpp | 4 +- src/broadphase/broadphase_bruteforce.cpp | 4 +- .../broadphase_collision_manager.cpp | 4 +- ...roadphase_continuous_collision_manager.cpp | 4 +- .../broadphase_dynamic_AABB_tree.cpp | 4 +- .../broadphase_dynamic_AABB_tree_array.cpp | 4 +- src/broadphase/broadphase_interval_tree.cpp | 4 +- src/broadphase/broadphase_spatialhash.cpp | 4 +- src/broadphase/detail/interval_tree.cpp | 4 +- src/broadphase/detail/interval_tree_node.cpp | 2 +- src/broadphase/detail/morton.cpp | 5 +- src/broadphase/detail/simple_interval.cpp | 2 +- src/broadphase/detail/spatial_hash.cpp | 4 +- src/common/detail/profiler.cpp | 8 +- src/geometry/bvh/BVH_utility.cpp | 2 + src/geometry/collision_geometry.cpp | 2 +- src/geometry/octree/octree.cpp | 5 +- src/geometry/shape/box.cpp | 2 +- src/geometry/shape/capsule.cpp | 2 +- src/geometry/shape/cone.cpp | 2 +- src/geometry/shape/convex.cpp | 2 +- src/geometry/shape/cylinder.cpp | 2 +- src/geometry/shape/ellipsoid.cpp | 2 +- src/geometry/shape/halfspace.cpp | 3 +- src/geometry/shape/plane.cpp | 3 +- src/geometry/shape/shape_base.cpp | 2 +- src/geometry/shape/sphere.cpp | 2 +- src/geometry/shape/triangle_p.cpp | 2 +- src/geometry/shape/utility.cpp | 74 +- src/math/bv/AABB.cpp | 2 +- src/math/bv/OBB.cpp | 7 +- src/math/bv/OBBRSS.cpp | 3 +- src/math/bv/RSS.cpp | 8 +- src/math/bv/kDOP.cpp | 11 +- src/math/bv/kIOS.cpp | 4 +- src/math/bv/utility.cpp | 44 +- src/math/detail/polysolver.cpp | 98 +- src/math/detail/project.cpp | 100 +- src/math/geometry.cpp | 18 + src/math/motion/interp_motion.cpp | 2 +- src/math/motion/motion_base.cpp | 2 +- src/math/motion/screw_motion.cpp | 2 +- src/math/motion/spline_motion.cpp | 2 +- src/math/motion/taylor_model/interval.cpp | 4 +- .../motion/taylor_model/interval_matrix.cpp | 3 +- .../motion/taylor_model/interval_vector.cpp | 4 +- .../motion/taylor_model/taylor_matrix.cpp | 9 +- src/math/motion/taylor_model/taylor_model.cpp | 8 +- .../motion/taylor_model/taylor_vector.cpp | 6 +- .../motion/taylor_model/time_interval.cpp | 2 +- src/math/motion/translation_motion.cpp | 2 +- .../motion/triangle_motion_bound_visitor.cpp | 2 +- src/math/rng.cpp | 2 +- src/math/sampler/sampler_base.cpp | 2 +- src/math/sampler/sampler_se2.cpp | 2 +- src/math/sampler/sampler_se2_disk.cpp | 2 +- src/math/sampler/sampler_se3_euler.cpp | 2 +- src/math/sampler/sampler_se3_euler_ball.cpp | 2 +- src/math/sampler/sampler_se3_quat.cpp | 2 +- src/math/sampler/sampler_se3_quat_ball.cpp | 2 +- src/math/variance3.cpp | 2 +- src/narrowphase/collision.cpp | 2 + src/narrowphase/collision_object.cpp | 2 +- src/narrowphase/collision_request.cpp | 2 +- src/narrowphase/collision_result.cpp | 2 +- src/narrowphase/contact.cpp | 2 +- src/narrowphase/contact_point.cpp | 4 +- src/narrowphase/continuous_collision.cpp | 4 + .../continuous_collision_object.cpp | 2 +- .../continuous_collision_request.cpp | 2 +- .../continuous_collision_result.cpp | 2 +- src/narrowphase/cost_source.cpp | 2 +- .../detail/convexity_based_algorithm/epa.cpp | 2 +- .../detail/convexity_based_algorithm/gjk.cpp | 2 +- .../convexity_based_algorithm/gjk_libccd.cpp | 19 +- .../minkowski_diff.cpp | 2 +- src/narrowphase/detail/gjk_solver_indep.cpp | 2 +- src/narrowphase/detail/gjk_solver_libccd.cpp | 2 +- .../primitive_shape_algorithm/box_box.cpp | 5 + .../capsule_capsule.cpp | 16 +- .../primitive_shape_algorithm/halfspace.cpp | 12 + .../primitive_shape_algorithm/intersect.cpp | 100 +- .../primitive_shape_algorithm/plane.cpp | 11 + .../primitive_shape_algorithm/sphere_box.cpp | 20 +- .../sphere_capsule.cpp | 3 + .../sphere_cylinder.cpp | 28 +- .../sphere_sphere.cpp | 2 + .../sphere_triangle.cpp | 6 + .../triangle_distance.cpp | 102 +- .../collision_traversal_node_base.cpp | 2 +- .../mesh_collision_traversal_node.cpp | 12 +- ...sh_continuous_collision_traversal_node.cpp | 2 +- .../detail/traversal/collision_node.cpp | 5 + .../conservative_advancement_stack_data.cpp | 2 +- .../distance/distance_traversal_node_base.cpp | 2 +- ...onservative_advancement_traversal_node.cpp | 6 +- .../distance/mesh_distance_traversal_node.cpp | 9 +- .../detail/traversal/traversal_node_base.cpp | 2 +- .../detail/traversal/traversal_recurse.cpp | 7 + src/narrowphase/distance.cpp | 2 + src/narrowphase/distance_request.cpp | 2 +- src/narrowphase/distance_result.cpp | 2 +- 357 files changed, 4082 insertions(+), 4107 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e90e56b2a..fccf6a28a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,13 +31,7 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -if(APPLE) - cmake_minimum_required(VERSION 3.0.0) -elseif(MSVC) - cmake_minimum_required(VERSION 3.1.3) -else() - cmake_minimum_required(VERSION 2.8.12) -endif() +cmake_minimum_required(VERSION 3.7) project(fcl CXX) diff --git a/include/fcl/CMakeLists.txt b/include/fcl/CMakeLists.txt index 35c467026..df229bb78 100644 --- a/include/fcl/CMakeLists.txt +++ b/include/fcl/CMakeLists.txt @@ -35,8 +35,29 @@ set(GENERATED_FILE_MARKER "GENERATED FILE DO NOT EDIT") configure_file(config.h.in config.h @ONLY) +set(FCL_EXPORT_MACRO_NAME "FCL_API") +if(MSVC) + set(EXPORT_INSTANTIATION_TEMPLATE_MACROS " +#ifdef ${PROJECT_NAME}_EXPORTS + /* We are building this library */ +# define FCL_EXTERN_TEMPLATE_API +# define FCL_INSTANTIATION_DEF_API ${FCL_EXPORT_MACRO_NAME} +#else + /* We are using this library */ +# define FCL_EXTERN_TEMPLATE_API ${FCL_EXPORT_MACRO_NAME} +# define FCL_INSTANTIATION_DEF_API +#endif +") +else() + set(EXPORT_INSTANTIATION_TEMPLATE_MACROS " +#define FCL_EXTERN_TEMPLATE_API ${FCL_EXPORT_MACRO_NAME} +#define FCL_INSTANTIATION_DEF_API +") +endif() generate_export_header(${PROJECT_NAME} + EXPORT_MACRO_NAME ${FCL_EXPORT_MACRO_NAME} EXPORT_FILE_NAME export.h + CUSTOM_CONTENT_FROM_VARIABLE EXPORT_INSTANTIATION_TEMPLATE_MACROS ) get_filename_component(PARENT_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}" diff --git a/include/fcl/broadphase/broadphase_SSaP-inl.h b/include/fcl/broadphase/broadphase_SSaP-inl.h index ad859ea86..526b620c0 100644 --- a/include/fcl/broadphase/broadphase_SSaP-inl.h +++ b/include/fcl/broadphase/broadphase_SSaP-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SSaPCollisionManager; +class FCL_EXTERN_TEMPLATE_API SSaPCollisionManager; /** @brief Functor sorting objects according to the AABB lower x bound */ template @@ -85,7 +85,7 @@ struct SortByZLow /** @brief Dummy collision object with a point AABB */ template -class FCL_EXPORT DummyCollisionObject : public CollisionObject +class DummyCollisionObject : public CollisionObject { public: DummyCollisionObject(const AABB& aabb_) : CollisionObject(std::shared_ptr>()) diff --git a/include/fcl/broadphase/broadphase_SSaP.h b/include/fcl/broadphase/broadphase_SSaP.h index b0cc0a9df..1d0225e76 100644 --- a/include/fcl/broadphase/broadphase_SSaP.h +++ b/include/fcl/broadphase/broadphase_SSaP.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -44,9 +44,9 @@ namespace fcl { -/// @brief Simple SAP collision manager +/// @brief Simple SAP collision manager template -class FCL_EXPORT SSaPCollisionManager : public BroadPhaseCollisionManager +class SSaPCollisionManager : public BroadPhaseCollisionManager { public: SSaPCollisionManager(); @@ -89,7 +89,7 @@ class FCL_EXPORT SSaPCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; @@ -97,13 +97,13 @@ class FCL_EXPORT SSaPCollisionManager : public BroadPhaseCollisionManager /// @brief check collision between one object and a list of objects, return value is whether stop is possible bool checkColl(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const; - + /// @brief check distance between one object and a list of objects, return value is whether stop is possible bool checkDis(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; - + bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; static size_t selectOptimalAxis( diff --git a/include/fcl/broadphase/broadphase_SaP-inl.h b/include/fcl/broadphase/broadphase_SaP-inl.h index 7d201c7d3..a4ea8cac4 100644 --- a/include/fcl/broadphase/broadphase_SaP-inl.h +++ b/include/fcl/broadphase/broadphase_SaP-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SaPCollisionManager; +class FCL_EXTERN_TEMPLATE_API SaPCollisionManager; //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_SaP.h b/include/fcl/broadphase/broadphase_SaP.h index 8a6b611ab..e5b210c27 100644 --- a/include/fcl/broadphase/broadphase_SaP.h +++ b/include/fcl/broadphase/broadphase_SaP.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -48,7 +48,7 @@ namespace fcl /// @brief Rigorous SAP collision manager template -class FCL_EXPORT SaPCollisionManager : public BroadPhaseCollisionManager +class SaPCollisionManager : public BroadPhaseCollisionManager { public: @@ -103,7 +103,7 @@ class FCL_EXPORT SaPCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; @@ -119,10 +119,10 @@ class FCL_EXPORT SaPCollisionManager : public BroadPhaseCollisionManager struct SaPPair; /// @brief Functor to help unregister one object - class FCL_EXPORT isUnregistered; + class isUnregistered; /// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away) - class FCL_EXPORT isNotValidPair; + class isNotValidPair; void update_(SaPAABB* updated_aabb); @@ -130,7 +130,7 @@ class FCL_EXPORT SaPCollisionManager : public BroadPhaseCollisionManager /// @brief End point list for x, y, z coordinates EndPoint* elist[3]; - + /// @brief vector version of elist, for acceleration std::vector velist[3]; @@ -215,7 +215,7 @@ struct SaPCollisionManager::SaPPair /// @brief Functor to help unregister one object template -class FCL_EXPORT SaPCollisionManager::isUnregistered +class SaPCollisionManager::isUnregistered { CollisionObject* obj; @@ -227,7 +227,7 @@ class FCL_EXPORT SaPCollisionManager::isUnregistered /// @brief Functor to help remove collision pairs no longer valid (i.e., should be culled away) template -class FCL_EXPORT SaPCollisionManager::isNotValidPair +class SaPCollisionManager::isNotValidPair { CollisionObject* obj1; CollisionObject* obj2; diff --git a/include/fcl/broadphase/broadphase_bruteforce-inl.h b/include/fcl/broadphase/broadphase_bruteforce-inl.h index 0c92ace3f..1958e9748 100644 --- a/include/fcl/broadphase/broadphase_bruteforce-inl.h +++ b/include/fcl/broadphase/broadphase_bruteforce-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -46,7 +46,7 @@ namespace fcl { //============================================================================== extern template -class FCL_EXPORT NaiveCollisionManager; +class FCL_EXTERN_TEMPLATE_API NaiveCollisionManager; //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_bruteforce.h b/include/fcl/broadphase/broadphase_bruteforce.h index f8ce6c52c..322576b02 100644 --- a/include/fcl/broadphase/broadphase_bruteforce.h +++ b/include/fcl/broadphase/broadphase_bruteforce.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -46,7 +46,7 @@ namespace fcl /// @brief Brute force N-body collision manager template -class FCL_EXPORT NaiveCollisionManager : public BroadPhaseCollisionManager +class NaiveCollisionManager : public BroadPhaseCollisionManager { public: NaiveCollisionManager(); @@ -92,7 +92,7 @@ class FCL_EXPORT NaiveCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_collision_manager-inl.h b/include/fcl/broadphase/broadphase_collision_manager-inl.h index e69f86c4f..7ab9b1fde 100644 --- a/include/fcl/broadphase/broadphase_collision_manager-inl.h +++ b/include/fcl/broadphase/broadphase_collision_manager-inl.h @@ -46,7 +46,7 @@ namespace fcl { //============================================================================== extern template -class FCL_EXPORT BroadPhaseCollisionManager; +class FCL_EXTERN_TEMPLATE_API BroadPhaseCollisionManager; //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_collision_manager.h b/include/fcl/broadphase/broadphase_collision_manager.h index c372c7a43..6c4306a69 100644 --- a/include/fcl/broadphase/broadphase_collision_manager.h +++ b/include/fcl/broadphase/broadphase_collision_manager.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -63,7 +63,7 @@ using DistanceCallBack = bool (*)( /// collision/distance between N objects. Also support self collision, self /// distance and collision/distance with another M objects. template -class FCL_EXPORT BroadPhaseCollisionManager +class BroadPhaseCollisionManager { public: BroadPhaseCollisionManager(); @@ -117,7 +117,7 @@ class FCL_EXPORT BroadPhaseCollisionManager /// @brief whether the manager is empty virtual bool empty() const = 0; - + /// @brief the number of objects managed by the manager virtual size_t size() const = 0; diff --git a/include/fcl/broadphase/broadphase_continuous_collision_manager-inl.h b/include/fcl/broadphase/broadphase_continuous_collision_manager-inl.h index 05ff0f804..e5033b793 100644 --- a/include/fcl/broadphase/broadphase_continuous_collision_manager-inl.h +++ b/include/fcl/broadphase/broadphase_continuous_collision_manager-inl.h @@ -46,7 +46,7 @@ namespace fcl { //============================================================================== extern template -class FCL_EXPORT BroadPhaseContinuousCollisionManager; +class FCL_EXTERN_TEMPLATE_API BroadPhaseContinuousCollisionManager; //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_continuous_collision_manager.h b/include/fcl/broadphase/broadphase_continuous_collision_manager.h index d0a043572..f9950eb46 100644 --- a/include/fcl/broadphase/broadphase_continuous_collision_manager.h +++ b/include/fcl/broadphase/broadphase_continuous_collision_manager.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -63,7 +63,7 @@ using ContinuousDistanceCallBack = bool (*)( /// accelerate the continuous collision/distance between N objects. Also support /// self collision, self distance and collision/distance with another M objects. template -class FCL_EXPORT BroadPhaseContinuousCollisionManager +class BroadPhaseContinuousCollisionManager { public: BroadPhaseContinuousCollisionManager(); @@ -117,7 +117,7 @@ class FCL_EXPORT BroadPhaseContinuousCollisionManager /// @brief whether the manager is empty virtual bool empty() const = 0; - + /// @brief the number of objects managed by the manager virtual size_t size() const = 0; }; diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h index 1a7341b5c..f792f563b 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -50,7 +50,7 @@ namespace fcl { //============================================================================== extern template -class FCL_EXPORT DynamicAABBTreeCollisionManager; +class FCL_EXTERN_TEMPLATE_API DynamicAABBTreeCollisionManager; namespace detail { @@ -59,7 +59,6 @@ namespace dynamic_AABB_tree { #if FCL_HAVE_OCTOMAP //============================================================================== template -FCL_EXPORT bool collisionRecurse_( typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, @@ -171,7 +170,6 @@ bool collisionRecurse_( //============================================================================== template -FCL_EXPORT bool collisionRecurse_( typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, @@ -278,7 +276,6 @@ bool collisionRecurse_( //============================================================================== template -FCL_EXPORT bool distanceRecurse_( typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, @@ -369,7 +366,6 @@ bool distanceRecurse_( //============================================================================== template -FCL_EXPORT bool collisionRecurse( typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, @@ -387,7 +383,6 @@ bool collisionRecurse( //============================================================================== template -FCL_EXPORT bool distanceRecurse_( typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, @@ -477,7 +472,6 @@ bool distanceRecurse_( //============================================================================== template -FCL_EXPORT bool distanceRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3& tf2, void* cdata, DistanceCallBack callback, S& min_dist) { if(tf2.linear().isIdentity()) @@ -490,7 +484,6 @@ bool distanceRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNod //============================================================================== template -FCL_EXPORT bool collisionRecurse( typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, @@ -524,7 +517,6 @@ bool collisionRecurse( //============================================================================== template -FCL_EXPORT bool collisionRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, CollisionCallBack callback) { if(root->isLeaf()) @@ -548,7 +540,6 @@ bool collisionRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNo //============================================================================== template -FCL_EXPORT bool selfCollisionRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, CollisionCallBack callback) { if(root->isLeaf()) return false; @@ -567,7 +558,6 @@ bool selfCollisionRecurse(typename DynamicAABBTreeCollisionManager::DynamicAA //============================================================================== template -FCL_EXPORT bool distanceRecurse( typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, @@ -656,7 +646,6 @@ bool distanceRecurse( //============================================================================== template -FCL_EXPORT bool distanceRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root, CollisionObject* query, void* cdata, DistanceCallBack callback, S& min_dist) { if(root->isLeaf()) @@ -702,7 +691,6 @@ bool distanceRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNod //============================================================================== template -FCL_EXPORT bool selfDistanceRecurse(typename DynamicAABBTreeCollisionManager::DynamicAABBNode* root, void* cdata, DistanceCallBack callback, S& min_dist) { if(root->isLeaf()) return false; @@ -725,7 +713,6 @@ bool selfDistanceRecurse(typename DynamicAABBTreeCollisionManager::DynamicAAB //============================================================================== template -FCL_EXPORT DynamicAABBTreeCollisionManager::DynamicAABBTreeCollisionManager() : tree_topdown_balance_threshold(dtree.bu_threshold), tree_topdown_level(dtree.topdown_level) @@ -744,7 +731,6 @@ DynamicAABBTreeCollisionManager::DynamicAABBTreeCollisionManager() //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::registerObjects( const std::vector*>& other_objs) { @@ -777,7 +763,6 @@ void DynamicAABBTreeCollisionManager::registerObjects( //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) { DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj); @@ -786,7 +771,6 @@ void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj) { DynamicAABBNode* node = table[obj]; @@ -796,7 +780,6 @@ void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* ob //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::setup() { if(!setup_) @@ -822,7 +805,6 @@ void DynamicAABBTreeCollisionManager::setup() //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::update() { for(auto it = table.cbegin(); it != table.cend(); ++it) @@ -840,7 +822,6 @@ void DynamicAABBTreeCollisionManager::update() //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj) { const auto it = table.find(updated_obj); @@ -855,7 +836,6 @@ void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) { update_(updated_obj); @@ -864,7 +844,6 @@ void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::update(const std::vector*>& updated_objs) { for(size_t i = 0, size = updated_objs.size(); i < size; ++i) @@ -874,7 +853,6 @@ void DynamicAABBTreeCollisionManager::update(const std::vector -FCL_EXPORT void DynamicAABBTreeCollisionManager::clear() { dtree.clear(); @@ -883,7 +861,6 @@ void DynamicAABBTreeCollisionManager::clear() //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::getObjects(std::vector*>& objs) const { objs.resize(this->size()); @@ -892,7 +869,6 @@ void DynamicAABBTreeCollisionManager::getObjects(std::vector -FCL_EXPORT void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; @@ -918,7 +894,6 @@ void DynamicAABBTreeCollisionManager::collide(CollisionObject* obj, void* //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { if(size() == 0) return; @@ -945,7 +920,6 @@ void DynamicAABBTreeCollisionManager::distance(CollisionObject* obj, void* //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::collide(void* cdata, CollisionCallBack callback) const { if(size() == 0) return; @@ -954,7 +928,6 @@ void DynamicAABBTreeCollisionManager::collide(void* cdata, CollisionCallBack< //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const { if(size() == 0) return; @@ -964,7 +937,6 @@ void DynamicAABBTreeCollisionManager::distance(void* cdata, DistanceCallBack< //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); @@ -974,7 +946,6 @@ void DynamicAABBTreeCollisionManager::collide(BroadPhaseCollisionManager* //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { DynamicAABBTreeCollisionManager* other_manager = static_cast(other_manager_); @@ -985,7 +956,6 @@ void DynamicAABBTreeCollisionManager::distance(BroadPhaseCollisionManager* //============================================================================== template -FCL_EXPORT bool DynamicAABBTreeCollisionManager::empty() const { return dtree.empty(); @@ -993,7 +963,6 @@ bool DynamicAABBTreeCollisionManager::empty() const //============================================================================== template -FCL_EXPORT size_t DynamicAABBTreeCollisionManager::size() const { return dtree.size(); @@ -1001,7 +970,6 @@ size_t DynamicAABBTreeCollisionManager::size() const //============================================================================== template -FCL_EXPORT const detail::HierarchyTree>& DynamicAABBTreeCollisionManager::getTree() const { diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h index 7ee6a333f..e6cfc8902 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -51,7 +51,7 @@ namespace fcl { template -class FCL_EXPORT DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager +class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager { public: @@ -71,7 +71,7 @@ class FCL_EXPORT DynamicAABBTreeCollisionManager : public BroadPhaseCollisionMan /// @brief add objects to the manager void registerObjects(const std::vector*>& other_objs); - + /// @brief add one object to the manager void registerObject(CollisionObject* obj); @@ -113,10 +113,10 @@ class FCL_EXPORT DynamicAABBTreeCollisionManager : public BroadPhaseCollisionMan /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; - + /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index 3cec23cbc..1080a42cc 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -49,7 +49,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT DynamicAABBTreeCollisionManager_Array; +class FCL_EXTERN_TEMPLATE_API DynamicAABBTreeCollisionManager_Array; namespace detail { @@ -61,7 +61,6 @@ namespace dynamic_AABB_tree_array //============================================================================== template -FCL_EXPORT bool collisionRecurse_( typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, @@ -175,7 +174,6 @@ bool collisionRecurse_( //============================================================================== template -FCL_EXPORT bool collisionRecurse_( typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, @@ -284,7 +282,6 @@ bool collisionRecurse_( //============================================================================== template -FCL_EXPORT bool distanceRecurse_(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3& tf2, void* cdata, DistanceCallBack callback, S& min_dist) { typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; @@ -368,7 +365,6 @@ bool distanceRecurse_(typename DynamicAABBTreeCollisionManager_Array::Dynamic //============================================================================== template -FCL_EXPORT bool distanceRecurse_( typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, @@ -464,7 +460,6 @@ bool distanceRecurse_( //============================================================================== template -FCL_EXPORT bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id, void* cdata, CollisionCallBack callback) @@ -498,7 +493,6 @@ bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array::Dynamic //============================================================================== template -FCL_EXPORT bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, CollisionCallBack callback) { typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; @@ -523,7 +517,6 @@ bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array::Dynamic //============================================================================== template -FCL_EXPORT bool selfCollisionRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, CollisionCallBack callback) { typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; @@ -543,7 +536,6 @@ bool selfCollisionRecurse(typename DynamicAABBTreeCollisionManager_Array::Dyn //============================================================================== template -FCL_EXPORT bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes2, size_t root2_id, void* cdata, DistanceCallBack callback, S& min_dist) @@ -631,7 +623,6 @@ bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicA //============================================================================== template -FCL_EXPORT bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, CollisionObject* query, void* cdata, DistanceCallBack callback, S& min_dist) { typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; @@ -678,7 +669,6 @@ bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicA //============================================================================== template -FCL_EXPORT bool selfDistanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes, size_t root_id, void* cdata, DistanceCallBack callback, S& min_dist) { typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root = nodes + root_id; @@ -701,7 +691,6 @@ bool selfDistanceRecurse(typename DynamicAABBTreeCollisionManager_Array::Dyna //============================================================================== template -FCL_EXPORT bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3& tf2, void* cdata, CollisionCallBack callback) { if(tf2.linear().isIdentity()) @@ -712,7 +701,6 @@ bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array::Dynamic //============================================================================== template -FCL_EXPORT bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const typename OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3& tf2, void* cdata, DistanceCallBack callback, S& min_dist) { if(tf2.linear().isIdentity()) @@ -729,7 +717,6 @@ bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array::DynamicA //============================================================================== template -FCL_EXPORT DynamicAABBTreeCollisionManager_Array::DynamicAABBTreeCollisionManager_Array() : tree_topdown_balance_threshold(dtree.bu_threshold), tree_topdown_level(dtree.topdown_level) @@ -748,7 +735,6 @@ DynamicAABBTreeCollisionManager_Array::DynamicAABBTreeCollisionManager_Array( //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::registerObjects( const std::vector*>& other_objs) { @@ -781,7 +767,6 @@ void DynamicAABBTreeCollisionManager_Array::registerObjects( //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::registerObject(CollisionObject* obj) { size_t node = dtree.insert(obj->getAABB(), obj); @@ -790,7 +775,6 @@ void DynamicAABBTreeCollisionManager_Array::registerObject(CollisionObject //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::unregisterObject(CollisionObject* obj) { size_t node = table[obj]; @@ -800,7 +784,6 @@ void DynamicAABBTreeCollisionManager_Array::unregisterObject(CollisionObject< //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::setup() { if(!setup_) @@ -826,7 +809,6 @@ void DynamicAABBTreeCollisionManager_Array::setup() //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::update() { for(auto it = table.cbegin(), end = table.cend(); it != end; ++it) @@ -844,7 +826,6 @@ void DynamicAABBTreeCollisionManager_Array::update() //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::update_(CollisionObject* updated_obj) { const auto it = table.find(updated_obj); @@ -859,7 +840,6 @@ void DynamicAABBTreeCollisionManager_Array::update_(CollisionObject* updat //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::update(CollisionObject* updated_obj) { update_(updated_obj); @@ -868,7 +848,6 @@ void DynamicAABBTreeCollisionManager_Array::update(CollisionObject* update //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::update(const std::vector*>& updated_objs) { for(size_t i = 0, size = updated_objs.size(); i < size; ++i) @@ -878,7 +857,6 @@ void DynamicAABBTreeCollisionManager_Array::update(const std::vector -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::clear() { dtree.clear(); @@ -887,7 +865,6 @@ void DynamicAABBTreeCollisionManager_Array::clear() //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::getObjects(std::vector*>& objs) const { objs.resize(this->size()); @@ -896,7 +873,6 @@ void DynamicAABBTreeCollisionManager_Array::getObjects(std::vector -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const { if(size() == 0) return; @@ -922,7 +898,6 @@ void DynamicAABBTreeCollisionManager_Array::collide(CollisionObject* obj, //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const { if(size() == 0) return; @@ -949,7 +924,6 @@ void DynamicAABBTreeCollisionManager_Array::distance(CollisionObject* obj, //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::collide(void* cdata, CollisionCallBack callback) const { if(size() == 0) return; @@ -958,7 +932,6 @@ void DynamicAABBTreeCollisionManager_Array::collide(void* cdata, CollisionCal //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::distance(void* cdata, DistanceCallBack callback) const { if(size() == 0) return; @@ -968,7 +941,6 @@ void DynamicAABBTreeCollisionManager_Array::distance(void* cdata, DistanceCal //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const { DynamicAABBTreeCollisionManager_Array* other_manager = static_cast(other_manager_); @@ -978,7 +950,6 @@ void DynamicAABBTreeCollisionManager_Array::collide(BroadPhaseCollisionManage //============================================================================== template -FCL_EXPORT void DynamicAABBTreeCollisionManager_Array::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const { DynamicAABBTreeCollisionManager_Array* other_manager = static_cast(other_manager_); @@ -989,7 +960,6 @@ void DynamicAABBTreeCollisionManager_Array::distance(BroadPhaseCollisionManag //============================================================================== template -FCL_EXPORT bool DynamicAABBTreeCollisionManager_Array::empty() const { return dtree.empty(); @@ -997,7 +967,6 @@ bool DynamicAABBTreeCollisionManager_Array::empty() const //============================================================================== template -FCL_EXPORT size_t DynamicAABBTreeCollisionManager_Array::size() const { return dtree.size(); @@ -1005,7 +974,6 @@ size_t DynamicAABBTreeCollisionManager_Array::size() const //============================================================================== template -FCL_EXPORT const detail::implementation_array::HierarchyTree>& DynamicAABBTreeCollisionManager_Array::getTree() const { diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h index 3d535f822..58b1432b9 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -52,7 +52,7 @@ namespace fcl { template -class FCL_EXPORT DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager +class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager { public: @@ -67,12 +67,12 @@ class FCL_EXPORT DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollis bool octree_as_geometry_collide; bool octree_as_geometry_distance; - + DynamicAABBTreeCollisionManager_Array(); /// @brief add objects to the manager void registerObjects(const std::vector*>& other_objs); - + /// @brief add one object to the manager void registerObject(CollisionObject* obj); @@ -114,10 +114,10 @@ class FCL_EXPORT DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollis /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; - + /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_interval_tree-inl.h b/include/fcl/broadphase/broadphase_interval_tree-inl.h index dcc4f6d79..f7f6f95e9 100644 --- a/include/fcl/broadphase/broadphase_interval_tree-inl.h +++ b/include/fcl/broadphase/broadphase_interval_tree-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT IntervalTreeCollisionManager; +class FCL_EXTERN_TEMPLATE_API IntervalTreeCollisionManager; //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_interval_tree.h b/include/fcl/broadphase/broadphase_interval_tree.h index 79c5467c0..b7b174d0c 100644 --- a/include/fcl/broadphase/broadphase_interval_tree.h +++ b/include/fcl/broadphase/broadphase_interval_tree.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -48,7 +48,7 @@ namespace fcl /// @brief Collision manager based on interval tree template -class FCL_EXPORT IntervalTreeCollisionManager : public BroadPhaseCollisionManager +class IntervalTreeCollisionManager : public BroadPhaseCollisionManager { public: IntervalTreeCollisionManager(); @@ -99,7 +99,7 @@ class FCL_EXPORT IntervalTreeCollisionManager : public BroadPhaseCollisionManage /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; @@ -147,7 +147,7 @@ using IntervalTreeCollisionManagerd = IntervalTreeCollisionManager; /// @brief SAP end point template -struct FCL_EXPORT IntervalTreeCollisionManager::EndPoint +struct IntervalTreeCollisionManager::EndPoint { /// @brief object related with the end point CollisionObject* obj; @@ -163,7 +163,7 @@ struct FCL_EXPORT IntervalTreeCollisionManager::EndPoint /// @brief Extention interval tree's interval to SAP interval, adding more information template -struct FCL_EXPORT IntervalTreeCollisionManager::SAPInterval : public detail::SimpleInterval +struct IntervalTreeCollisionManager::SAPInterval : public detail::SimpleInterval { CollisionObject* obj; diff --git a/include/fcl/broadphase/broadphase_spatialhash-inl.h b/include/fcl/broadphase/broadphase_spatialhash-inl.h index 12551ce08..85c3a6a0f 100644 --- a/include/fcl/broadphase/broadphase_spatialhash-inl.h +++ b/include/fcl/broadphase/broadphase_spatialhash-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SpatialHashingCollisionManager< +class FCL_EXTERN_TEMPLATE_API SpatialHashingCollisionManager< double, detail::SimpleHashTable< AABB, CollisionObject*, detail::SpatialHash>>; diff --git a/include/fcl/broadphase/broadphase_spatialhash.h b/include/fcl/broadphase/broadphase_spatialhash.h index a21d0417f..0cedef753 100644 --- a/include/fcl/broadphase/broadphase_spatialhash.h +++ b/include/fcl/broadphase/broadphase_spatialhash.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -53,7 +53,7 @@ namespace fcl template, CollisionObject*, detail::SpatialHash> > -class FCL_EXPORT SpatialHashingCollisionManager : public BroadPhaseCollisionManager +class SpatialHashingCollisionManager : public BroadPhaseCollisionManager { public: SpatialHashingCollisionManager( diff --git a/include/fcl/broadphase/detail/hierarchy_tree.h b/include/fcl/broadphase/detail/hierarchy_tree.h index 456314a5d..bdabe905b 100644 --- a/include/fcl/broadphase/detail/hierarchy_tree.h +++ b/include/fcl/broadphase/detail/hierarchy_tree.h @@ -55,7 +55,7 @@ namespace detail /// @brief Class for hierarchy tree structure template -class FCL_EXPORT HierarchyTree +class HierarchyTree { public: @@ -70,7 +70,7 @@ class FCL_EXPORT HierarchyTree HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); - + /// @brief Initialize the tree by a set of leaves using algorithm with a given level. void init(std::vector& leaves, int level = 0); @@ -80,10 +80,10 @@ class FCL_EXPORT HierarchyTree /// @brief Remove a leaf node void remove(NodeType* leaf); - /// @brief Clear the tree + /// @brief Clear the tree void clear(); - /// @brief Whether the tree is empty + /// @brief Whether the tree is empty bool empty() const; /// @brief Updates a `leaf` node. A use case is when the bounding volume @@ -102,10 +102,10 @@ class FCL_EXPORT HierarchyTree /// @brief update the tree when the bounding volume of a given leaf has changed bool update(NodeType* leaf, const BV& bv); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(NodeType* leaf, const BV& bv, const Vector3& vel, S margin); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(NodeType* leaf, const BV& bv, const Vector3& vel); /// @brief get the max height of the tree @@ -114,19 +114,19 @@ class FCL_EXPORT HierarchyTree /// @brief get the max depth of the tree size_t getMaxDepth() const; - /// @brief balance the tree from bottom + /// @brief balance the tree from bottom void balanceBottomup(); - /// @brief balance the tree from top + /// @brief balance the tree from top void balanceTopdown(); - - /// @brief balance the tree in an incremental way + + /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); - + /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner void refit(); - /// @brief extract all the leaves of the tree + /// @brief extract all the leaves of the tree void extractLeaves(const NodeType* root, std::vector& leaves) const; /// @brief number of leaves in the tree @@ -153,10 +153,10 @@ class FCL_EXPORT HierarchyTree } }; - /// @brief construct a tree for a set of leaves from bottom -- very heavy way + /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief construct a tree for a set of leaves from top + /// @brief construct a tree for a set of leaves from top NodeType* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief compute the maximum height of a subtree rooted from a given node @@ -184,24 +184,24 @@ class FCL_EXPORT HierarchyTree void init_1(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the node index. + /// we split the leaves into two parts with the same size simply using the node index. void init_2(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. void init_3(std::vector& leaves); - + NodeType* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32& split, int bits); NodeType* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32& split, int bits); NodeType* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief update one leaf node's bounding volume + /// @brief update one leaf node's bounding volume void update_(NodeType* leaf, const BV& bv); - /// @brief sort node n and its parent according to their memory position + /// @brief sort node n and its parent according to their memory position NodeType* sort(NodeType* n, NodeType*& r); - + /// @brief Insert a leaf node and also update its ancestors. Maintain the /// tree as a full binary tree (every interior node has exactly two children). /// Furthermore, adjust the BV of interior nodes so that each parent's BV @@ -219,13 +219,13 @@ class FCL_EXPORT HierarchyTree // adjusted. NodeType* removeLeaf(NodeType* const leaf); - /// @brief Delete all internal nodes and return all leaves nodes with given depth from root + /// @brief Delete all internal nodes and return all leaves nodes with given depth from root void fetchLeaves(NodeType* root, std::vector& leaves, int depth = -1); static size_t indexOf(NodeType* node); - /// @brief create one node (leaf or internal) - NodeType* createNode(NodeType* parent, + /// @brief create one node (leaf or internal) + NodeType* createNode(NodeType* parent, const BV& bv, void* data); @@ -233,7 +233,7 @@ class FCL_EXPORT HierarchyTree const BV& bv1, const BV& bv2, void* data); - + NodeType* createNode(NodeType* parent, void* data); @@ -250,11 +250,11 @@ class FCL_EXPORT HierarchyTree unsigned int opath; - /// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently. - NodeType* free_node; + /// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently. + NodeType* free_node; int max_lookahead_level; - + public: /// @brief decide which topdown algorithm to use int topdown_level; diff --git a/include/fcl/broadphase/detail/hierarchy_tree_array.h b/include/fcl/broadphase/detail/hierarchy_tree_array.h index b55f1e85e..a9ba63370 100644 --- a/include/fcl/broadphase/detail/hierarchy_tree_array.h +++ b/include/fcl/broadphase/detail/hierarchy_tree_array.h @@ -58,11 +58,11 @@ namespace implementation_array /// @brief Class for hierarchy tree structure template -class FCL_EXPORT HierarchyTree +class HierarchyTree { using S = typename BV::S; typedef NodeBase NodeType; - + struct SortByMorton { bool operator() (size_t a, size_t b) const @@ -99,22 +99,22 @@ class FCL_EXPORT HierarchyTree /// @brief Remove a leaf node void remove(size_t leaf); - /// @brief Clear the tree + /// @brief Clear the tree void clear(); - /// @brief Whether the tree is empty + /// @brief Whether the tree is empty bool empty() const; - - /// @brief update one leaf node + + /// @brief update one leaf node void update(size_t leaf, int lookahead_level = -1); /// @brief update the tree when the bounding volume of a given leaf has changed bool update(size_t leaf, const BV& bv); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vector3& vel, S margin); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vector3& vel); /// @brief get the max height of the tree @@ -123,19 +123,19 @@ class FCL_EXPORT HierarchyTree /// @brief get the max depth of the tree size_t getMaxDepth() const; - /// @brief balance the tree from bottom + /// @brief balance the tree from bottom void balanceBottomup(); - /// @brief balance the tree from top + /// @brief balance the tree from top void balanceTopdown(); - /// @brief balance the tree in an incremental way + /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner void refit(); - /// @brief extract all the leaves of the tree + /// @brief extract all the leaves of the tree void extractLeaves(size_t root, NodeType*& leaves) const; /// @brief number of leaves in the tree @@ -152,10 +152,10 @@ class FCL_EXPORT HierarchyTree private: - /// @brief construct a tree for a set of leaves from bottom -- very heavy way + /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(size_t* lbeg, size_t* lend); - - /// @brief construct a tree for a set of leaves from top + + /// @brief construct a tree for a set of leaves from top size_t topdown(size_t* lbeg, size_t* lend); /// @brief compute the maximum height of a subtree rooted from a given node @@ -183,7 +183,7 @@ class FCL_EXPORT HierarchyTree void init_1(NodeType* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the node index. + /// we split the leaves into two parts with the same size simply using the node index. void init_2(NodeType* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. @@ -195,17 +195,17 @@ class FCL_EXPORT HierarchyTree size_t mortonRecurse_2(size_t* lbeg, size_t* lend); - /// @brief update one leaf node's bounding volume + /// @brief update one leaf node's bounding volume void update_(size_t leaf, const BV& bv); - /// @brief Insert a leaf node and also update its ancestors + /// @brief Insert a leaf node and also update its ancestors void insertLeaf(size_t root, size_t leaf); /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted. - /// return the node with the smallest depth and is influenced by the remove operation + /// return the node with the smallest depth and is influenced by the remove operation size_t removeLeaf(size_t leaf); - /// @brief Delete all internal nodes and return all leaves nodes with given depth from root + /// @brief Delete all internal nodes and return all leaves nodes with given depth from root void fetchLeaves(size_t root, NodeType*& leaves, int depth = -1); size_t indexOf(size_t node); @@ -213,13 +213,13 @@ class FCL_EXPORT HierarchyTree size_t allocateNode(); /// @brief create one node (leaf or internal) - size_t createNode(size_t parent, + size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data); size_t createNode(size_t parent, - const BV& bv, + const BV& bv, void* data); size_t createNode(size_t parent, @@ -234,7 +234,7 @@ class FCL_EXPORT HierarchyTree NodeType* nodes; size_t n_nodes; size_t n_nodes_alloc; - + size_t n_leaves; size_t freelist; unsigned int opath; diff --git a/include/fcl/broadphase/detail/interval_tree-inl.h b/include/fcl/broadphase/detail/interval_tree-inl.h index 330b676e8..0ad41ee21 100644 --- a/include/fcl/broadphase/detail/interval_tree-inl.h +++ b/include/fcl/broadphase/detail/interval_tree-inl.h @@ -47,7 +47,7 @@ namespace detail { //============================================================================== extern template -class IntervalTree; +class FCL_EXTERN_TEMPLATE_API IntervalTree; //============================================================================== template diff --git a/include/fcl/broadphase/detail/interval_tree.h b/include/fcl/broadphase/detail/interval_tree.h index 6573520c9..a2a2f317f 100644 --- a/include/fcl/broadphase/detail/interval_tree.h +++ b/include/fcl/broadphase/detail/interval_tree.h @@ -51,7 +51,7 @@ namespace detail { /// right branch in searching for intervals but possibly come back /// and check the left branch as well. template -struct FCL_EXPORT it_recursion_node +struct it_recursion_node { public: IntervalTreeNode* start_node; @@ -65,11 +65,11 @@ using it_recursion_nodef = it_recursion_node; using it_recursion_noded = it_recursion_node; extern template -struct it_recursion_node; +struct FCL_EXTERN_TEMPLATE_API it_recursion_node; /// @brief Interval tree template -class FCL_EXPORT IntervalTree +class IntervalTree { public: @@ -113,7 +113,7 @@ class FCL_EXPORT IntervalTree /// @brief Inserts node into the tree as if it were a regular binary tree void recursiveInsert(IntervalTreeNode* node); - /// @brief recursively print a subtree + /// @brief recursively print a subtree void recursivePrint(IntervalTreeNode* node) const; /// @brief recursively find the node corresponding to the interval diff --git a/include/fcl/broadphase/detail/interval_tree_node-inl.h b/include/fcl/broadphase/detail/interval_tree_node-inl.h index 2badc0469..f06de88ff 100644 --- a/include/fcl/broadphase/detail/interval_tree_node-inl.h +++ b/include/fcl/broadphase/detail/interval_tree_node-inl.h @@ -47,7 +47,7 @@ namespace detail { //============================================================================== extern template -class FCL_EXPORT IntervalTreeNode; +class FCL_EXTERN_TEMPLATE_API IntervalTreeNode; //============================================================================== template diff --git a/include/fcl/broadphase/detail/interval_tree_node.h b/include/fcl/broadphase/detail/interval_tree_node.h index 537adab03..9d60e6e8d 100644 --- a/include/fcl/broadphase/detail/interval_tree_node.h +++ b/include/fcl/broadphase/detail/interval_tree_node.h @@ -48,11 +48,11 @@ namespace detail { template -class FCL_EXPORT IntervalTree; +class IntervalTree; /// @brief The node for interval tree template -class FCL_EXPORT IntervalTreeNode +class IntervalTreeNode { public: @@ -60,7 +60,7 @@ class FCL_EXPORT IntervalTreeNode friend class IntervalTree; friend class IntervalTree; - + /// @brief Create an empty node IntervalTreeNode(); @@ -83,7 +83,7 @@ class FCL_EXPORT IntervalTreeNode S max_high; /// @brief red or black node: if red = false then the node is black - bool red; + bool red; IntervalTreeNode* left; diff --git a/include/fcl/broadphase/detail/morton-inl.h b/include/fcl/broadphase/detail/morton-inl.h index cb23207b3..73521786e 100644 --- a/include/fcl/broadphase/detail/morton-inl.h +++ b/include/fcl/broadphase/detail/morton-inl.h @@ -47,15 +47,16 @@ namespace detail { //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API uint32 quantize(double x, uint32 n); //============================================================================== extern template -struct morton_functor; +struct FCL_EXTERN_TEMPLATE_API morton_functor; //============================================================================== extern template -struct morton_functor; +struct FCL_EXTERN_TEMPLATE_API morton_functor; //============================================================================== template diff --git a/include/fcl/broadphase/detail/morton.h b/include/fcl/broadphase/detail/morton.h index 6b430c434..ebbc1c718 100644 --- a/include/fcl/broadphase/detail/morton.h +++ b/include/fcl/broadphase/detail/morton.h @@ -52,15 +52,14 @@ namespace detail { template -FCL_EXPORT uint32 quantize(S x, uint32 n); /// @brief compute 30 bit morton code -FCL_EXPORT +FCL_API uint32 morton_code(uint32 x, uint32 y, uint32 z); /// @brief compute 60 bit morton code -FCL_EXPORT +FCL_API uint64 morton_code60(uint32 x, uint32 y, uint32 z); /// @brief Functor to compute the morton code for a given AABB @@ -68,11 +67,11 @@ uint64 morton_code60(uint32 x, uint32 y, uint32 z); /// a 30- or 60-bit code, respectively, and for `std::bitset` where /// N is the length of the code and must be a multiple of 3. template -struct FCL_EXPORT morton_functor {}; +struct morton_functor {}; /// @brief Functor to compute 30 bit morton code for a given AABB template -struct FCL_EXPORT morton_functor +struct morton_functor { morton_functor(const AABB& bbox); @@ -89,7 +88,7 @@ using morton_functoru32d = morton_functor; /// @brief Functor to compute 60 bit morton code for a given AABB template -struct FCL_EXPORT morton_functor +struct morton_functor { morton_functor(const AABB& bbox); @@ -107,7 +106,7 @@ using morton_functoru64d = morton_functor; /// @brief Functor to compute N bit morton code for a given AABB /// N must be a multiple of 3. template -struct FCL_EXPORT morton_functor> +struct morton_functor> { static_assert(N%3==0, "Number of bits must be a multiple of 3"); diff --git a/include/fcl/broadphase/detail/simple_hash_table.h b/include/fcl/broadphase/detail/simple_hash_table.h index 2708d02a9..d45d3c98a 100644 --- a/include/fcl/broadphase/detail/simple_hash_table.h +++ b/include/fcl/broadphase/detail/simple_hash_table.h @@ -52,7 +52,7 @@ namespace detail /// @brief A simple hash table implemented as multiple buckets. HashFnc is any /// extended hash function: HashFnc(key) = {index1, index2, ..., } template -class FCL_EXPORT SimpleHashTable +class SimpleHashTable { protected: typedef std::list Bin; diff --git a/include/fcl/broadphase/detail/simple_interval-inl.h b/include/fcl/broadphase/detail/simple_interval-inl.h index 4daafd9ac..c136bb345 100644 --- a/include/fcl/broadphase/detail/simple_interval-inl.h +++ b/include/fcl/broadphase/detail/simple_interval-inl.h @@ -45,7 +45,7 @@ namespace detail { //============================================================================== extern template -struct SimpleInterval; +struct FCL_EXTERN_TEMPLATE_API SimpleInterval; //============================================================================== template diff --git a/include/fcl/broadphase/detail/simple_interval.h b/include/fcl/broadphase/detail/simple_interval.h index ee8cbcadb..0c5e8e253 100644 --- a/include/fcl/broadphase/detail/simple_interval.h +++ b/include/fcl/broadphase/detail/simple_interval.h @@ -49,11 +49,11 @@ namespace detail /// @brief Interval trees implemented using red-black-trees as described in /// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest. template -struct FCL_EXPORT SimpleInterval +struct SimpleInterval { public: virtual ~SimpleInterval(); - + virtual void print(); /// @brief interval is defined as [low, high] diff --git a/include/fcl/broadphase/detail/sparse_hash_table.h b/include/fcl/broadphase/detail/sparse_hash_table.h index 8f1a133e5..7d1f07aef 100644 --- a/include/fcl/broadphase/detail/sparse_hash_table.h +++ b/include/fcl/broadphase/detail/sparse_hash_table.h @@ -51,25 +51,25 @@ namespace detail { template -class FCL_EXPORT unordered_map_hash_table : public std::unordered_map {}; +class unordered_map_hash_table : public std::unordered_map {}; /// @brief A hash table implemented using unordered_map template class TableT = unordered_map_hash_table> -class FCL_EXPORT SparseHashTable +class SparseHashTable { protected: HashFnc h_; typedef std::list Bin; typedef TableT Table; - + Table table_; public: SparseHashTable(const HashFnc& h); /// @brief Init the hash table. The bucket size is dynamically decided. void init(size_t); - + /// @brief insert one key-value pair into the hash table void insert(Key key, Data value); diff --git a/include/fcl/broadphase/detail/spatial_hash-inl.h b/include/fcl/broadphase/detail/spatial_hash-inl.h index 1d4985f20..9eba06d63 100644 --- a/include/fcl/broadphase/detail/spatial_hash-inl.h +++ b/include/fcl/broadphase/detail/spatial_hash-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -45,7 +45,7 @@ namespace detail { //============================================================================== extern template -struct SpatialHash; +struct FCL_EXTERN_TEMPLATE_API SpatialHash; //============================================================================== template diff --git a/include/fcl/broadphase/detail/spatial_hash.h b/include/fcl/broadphase/detail/spatial_hash.h index b3f1cdd88..f3fdf7e15 100644 --- a/include/fcl/broadphase/detail/spatial_hash.h +++ b/include/fcl/broadphase/detail/spatial_hash.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -48,12 +48,12 @@ namespace detail /// @brief Spatial hash function: hash an AABB to a set of integer values template -struct FCL_EXPORT SpatialHash +struct SpatialHash { using S = S_; SpatialHash(const AABB& scene_limit_, S cell_size_); - + std::vector operator() (const AABB& aabb) const; private: diff --git a/include/fcl/common/detail/profiler.h b/include/fcl/common/detail/profiler.h index 76e3675b6..7d91706b3 100644 --- a/include/fcl/common/detail/profiler.h +++ b/include/fcl/common/detail/profiler.h @@ -59,7 +59,7 @@ namespace detail { /// external profiling tools in that it allows the user to count /// time spent in various bits of code (sub-function granularity) /// or count how many times certain pieces of code are executed. -class FCL_EXPORT Profiler +class FCL_API Profiler { public: // non-copyable @@ -68,13 +68,13 @@ class FCL_EXPORT Profiler /// @brief This instance will call Profiler::begin() when constructed and /// Profiler::end() when it goes out of scope. - class FCL_EXPORT ScopedBlock; + class FCL_API ScopedBlock; /// @brief This instance will call Profiler::start() when constructed and /// Profiler::stop() when it goes out of scope. /// If the profiler was already started, this block's constructor and /// destructor take no action - class FCL_EXPORT ScopedStart; + class FCL_API ScopedStart; /// @brief Return an instance of the class static Profiler& Instance(void); @@ -211,7 +211,7 @@ class FCL_EXPORT Profiler /// @brief This instance will call Profiler::begin() when constructed and /// Profiler::end() when it goes out of scope. -class FCL_EXPORT Profiler::ScopedBlock +class FCL_API Profiler::ScopedBlock { public: /// @brief Start counting time for the block named \e name of the profiler @@ -230,7 +230,7 @@ class FCL_EXPORT Profiler::ScopedBlock /// Profiler::stop() when it goes out of scope. /// If the profiler was already started, this block's constructor and /// destructor take no action -class FCL_EXPORT Profiler::ScopedStart +class FCL_API Profiler::ScopedStart { public: diff --git a/include/fcl/common/exception.h b/include/fcl/common/exception.h index 00da2ccfa..d55d352b8 100644 --- a/include/fcl/common/exception.h +++ b/include/fcl/common/exception.h @@ -46,7 +46,7 @@ namespace fcl { -class FCL_EXPORT Exception : public std::runtime_error +class FCL_API Exception : public std::runtime_error { public: diff --git a/include/fcl/common/time.h b/include/fcl/common/time.h index 7b040c518..9d4af9b9b 100644 --- a/include/fcl/common/time.h +++ b/include/fcl/common/time.h @@ -55,15 +55,15 @@ using point = std::chrono::system_clock::time_point; using duration = std::chrono::system_clock::duration; /// @brief Get the current time point -FCL_EXPORT +FCL_API point now(void); /// @brief Return the time duration representing a given number of seconds -FCL_EXPORT +FCL_API duration seconds(double sec); /// @brief Return the number of seconds that a time duration represents -FCL_EXPORT +FCL_API double seconds(const duration &d); } // namespace time diff --git a/include/fcl/common/types.h b/include/fcl/common/types.h index 53f49d711..bd9d5a6e0 100644 --- a/include/fcl/common/types.h +++ b/include/fcl/common/types.h @@ -140,7 +140,7 @@ inline std::shared_ptr<_Tp> make_aligned_shared(_Args&&... __args) // C++11 compatible version is available since Eigen 3.2.9 so we use this copy // for Eigen (< 3.2.9). template -class FCL_EXPORT aligned_allocator_cpp11 : public std::allocator +class aligned_allocator_cpp11 : public std::allocator { public: typedef std::size_t size_type; diff --git a/include/fcl/geometry/bvh/BVH_model.h b/include/fcl/geometry/bvh/BVH_model.h index 294d521e4..15849ee24 100644 --- a/include/fcl/geometry/bvh/BVH_model.h +++ b/include/fcl/geometry/bvh/BVH_model.h @@ -54,7 +54,7 @@ namespace fcl /// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh) template -class FCL_EXPORT BVHModel : public CollisionGeometry +class BVHModel : public CollisionGeometry { public: @@ -74,7 +74,7 @@ class FCL_EXPORT BVHModel : public CollisionGeometry /// @brief We provide getBV() and getNumBVs() because BVH may be compressed /// (in future), so we must provide some flexibility here - + /// @brief Access the bv giving the its index const BVNode& getBV(int id) const; @@ -147,7 +147,7 @@ class FCL_EXPORT BVHModel : public CollisionGeometry /// @brief Check the number of memory used int memUsage(int msg) const; - /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent + /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent /// BV node. When traversing the BVH, this can save one matrix transformation. void makeParentRelative(); @@ -211,7 +211,7 @@ class FCL_EXPORT BVHModel : public CollisionGeometry /// @brief Recursive kernel for hierarchy construction int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives); - /// @brief Recursive kernel for bottomup refitting + /// @brief Recursive kernel for bottomup refitting int recursiveRefitTree_bottomup(int bv_id); /// @recursively compute each bv's transform related to its parent. For diff --git a/include/fcl/geometry/bvh/BVH_utility-inl.h b/include/fcl/geometry/bvh/BVH_utility-inl.h index 2196ca486..f6e1ea5e8 100644 --- a/include/fcl/geometry/bvh/BVH_utility-inl.h +++ b/include/fcl/geometry/bvh/BVH_utility-inl.h @@ -47,17 +47,18 @@ namespace fcl //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void BVHExpand( BVHModel>& model, const Variance3* ucs, double r); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void BVHExpand( BVHModel>& model, const Variance3* ucs, double r); //============================================================================== template -FCL_EXPORT void BVHExpand(BVHModel& model, const Variance3* ucs, S r) { for(int i = 0; i < model.num_bvs; ++i) @@ -85,7 +86,6 @@ void BVHExpand(BVHModel& model, const Variance3* ucs, S r) //============================================================================== template -FCL_EXPORT void BVHExpand( BVHModel>& model, const Variance3* ucs, @@ -129,7 +129,6 @@ void BVHExpand( //============================================================================== template -FCL_EXPORT void BVHExpand( BVHModel>& model, const Variance3* ucs, diff --git a/include/fcl/geometry/bvh/BVH_utility.h b/include/fcl/geometry/bvh/BVH_utility.h index 68e3974b6..a0862228c 100644 --- a/include/fcl/geometry/bvh/BVH_utility.h +++ b/include/fcl/geometry/bvh/BVH_utility.h @@ -47,20 +47,17 @@ namespace fcl /// @brief Expand the BVH bounding boxes according to the variance matrix /// corresponding to the data stored within each BV node template -FCL_EXPORT void BVHExpand(BVHModel& model, const Variance3* ucs, S r); /// @brief Expand the BVH bounding boxes according to the corresponding variance /// information, for OBB template -FCL_EXPORT void BVHExpand( BVHModel>& model, const Variance3* ucs, S r = 1.0); /// @brief Expand the BVH bounding boxes according to the corresponding variance /// information, for RSS template -FCL_EXPORT void BVHExpand( BVHModel>& model, const Variance3* ucs, S r = 1.0); diff --git a/include/fcl/geometry/bvh/BV_node.h b/include/fcl/geometry/bvh/BV_node.h index 08dabbc0d..b87b9fd58 100644 --- a/include/fcl/geometry/bvh/BV_node.h +++ b/include/fcl/geometry/bvh/BV_node.h @@ -49,7 +49,7 @@ namespace fcl /// @brief A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and also the geometry data provided in BV template parameter. template -struct FCL_EXPORT BVNode : public BVNodeBase +struct BVNode : public BVNodeBase { using S = typename BV::S; diff --git a/include/fcl/geometry/bvh/BV_node_base.h b/include/fcl/geometry/bvh/BV_node_base.h index a64e67977..2b7f175a0 100644 --- a/include/fcl/geometry/bvh/BV_node_base.h +++ b/include/fcl/geometry/bvh/BV_node_base.h @@ -47,7 +47,7 @@ namespace fcl { /// @brief BVNodeBase encodes the tree structure for BVH -struct FCL_EXPORT BVNodeBase +struct FCL_API BVNodeBase { /// @brief An index for first child node or primitive /// If the value is positive, it is the index of the first child bv node @@ -59,7 +59,7 @@ struct FCL_EXPORT BVNodeBase /// we can obtain the primitive's index in original data indirectly. int first_primitive; - /// @brief The number of primitives belonging to the current node + /// @brief The number of primitives belonging to the current node int num_primitives; /// @brief Whether current node is a leaf node (i.e. contains a primitive index diff --git a/include/fcl/geometry/bvh/detail/BVH_front.h b/include/fcl/geometry/bvh/detail/BVH_front.h index 2415f91a9..9e3a1ddb9 100644 --- a/include/fcl/geometry/bvh/detail/BVH_front.h +++ b/include/fcl/geometry/bvh/detail/BVH_front.h @@ -52,7 +52,7 @@ namespace detail /// the traversal terminates while performing a query during a given time /// instance. The front list reflects the subset of a BVTT that is traversed for /// that particular proximity query. -struct FCL_EXPORT BVHFrontNode +struct FCL_API BVHFrontNode { /// @brief The nodes to start in the future, i.e. the wave front of the /// traversal tree. @@ -69,7 +69,7 @@ struct FCL_EXPORT BVHFrontNode using BVHFrontList = std::list; /// @brief Add new front node into the front list -FCL_EXPORT +FCL_API void updateFrontList(BVHFrontList* front_list, int b1, int b2); } // namespace detail diff --git a/include/fcl/geometry/bvh/detail/BV_fitter.h b/include/fcl/geometry/bvh/detail/BV_fitter.h index c78c52364..c31e2b8a3 100644 --- a/include/fcl/geometry/bvh/detail/BV_fitter.h +++ b/include/fcl/geometry/bvh/detail/BV_fitter.h @@ -53,7 +53,7 @@ namespace detail /// @brief The class for the default algorithm fitting a bounding volume to a set of points template -class FCL_EXPORT BVFitter : public BVFitterBase +class BVFitter : public BVFitterBase { public: diff --git a/include/fcl/geometry/bvh/detail/BV_fitter_base.h b/include/fcl/geometry/bvh/detail/BV_fitter_base.h index 3e10fbb33..6bc1db156 100644 --- a/include/fcl/geometry/bvh/detail/BV_fitter_base.h +++ b/include/fcl/geometry/bvh/detail/BV_fitter_base.h @@ -52,7 +52,7 @@ namespace detail /// @brief Interface for fitting a bv given the triangles or points inside it. template -class FCL_EXPORT BVFitterBase +class BVFitterBase { public: diff --git a/include/fcl/geometry/bvh/detail/BV_splitter.h b/include/fcl/geometry/bvh/detail/BV_splitter.h index 7480f7728..b15561026 100644 --- a/include/fcl/geometry/bvh/detail/BV_splitter.h +++ b/include/fcl/geometry/bvh/detail/BV_splitter.h @@ -62,7 +62,7 @@ enum SplitMethodType /// @brief A class describing the split rule that splits each BV node template -class FCL_EXPORT BVSplitter : public BVSplitterBase +class BVSplitter : public BVSplitterBase { public: diff --git a/include/fcl/geometry/bvh/detail/BV_splitter_base.h b/include/fcl/geometry/bvh/detail/BV_splitter_base.h index 893c2bedd..595b794da 100644 --- a/include/fcl/geometry/bvh/detail/BV_splitter_base.h +++ b/include/fcl/geometry/bvh/detail/BV_splitter_base.h @@ -53,7 +53,7 @@ namespace detail /// @brief Base interface for BV splitting algorithm template -class FCL_EXPORT BVSplitterBase +class BVSplitterBase { public: diff --git a/include/fcl/geometry/collision_geometry-inl.h b/include/fcl/geometry/collision_geometry-inl.h index b5695325e..3a0fded2f 100644 --- a/include/fcl/geometry/collision_geometry-inl.h +++ b/include/fcl/geometry/collision_geometry-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT CollisionGeometry; +class FCL_EXTERN_TEMPLATE_API CollisionGeometry; //============================================================================== template diff --git a/include/fcl/geometry/collision_geometry.h b/include/fcl/geometry/collision_geometry.h index 21fe84992..09d34b876 100644 --- a/include/fcl/geometry/collision_geometry.h +++ b/include/fcl/geometry/collision_geometry.h @@ -55,7 +55,7 @@ enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP /// @brief The geometry for the object for collision or distance computation template -class FCL_EXPORT CollisionGeometry +class CollisionGeometry { public: CollisionGeometry(); diff --git a/include/fcl/geometry/octree/octree-inl.h b/include/fcl/geometry/octree/octree-inl.h index b22b1e17d..aee9f2c23 100644 --- a/include/fcl/geometry/octree/octree-inl.h +++ b/include/fcl/geometry/octree/octree-inl.h @@ -49,10 +49,11 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT OcTree; +class FCL_EXTERN_TEMPLATE_API OcTree; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv); //============================================================================== diff --git a/include/fcl/geometry/octree/octree.h b/include/fcl/geometry/octree/octree.h index 58f73645d..908a01798 100644 --- a/include/fcl/geometry/octree/octree.h +++ b/include/fcl/geometry/octree/octree.h @@ -55,7 +55,7 @@ namespace fcl /// @brief Octree is one type of collision geometry which can encode uncertainty /// information in the sensor data. template -class FCL_EXPORT OcTree : public CollisionGeometry +class OcTree : public CollisionGeometry { private: std::shared_ptr tree; @@ -119,7 +119,7 @@ class FCL_EXPORT OcTree : public CollisionGeometry /// @return const ptr to child number childIdx of node const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const; - + /// @brief return true if the child at childIdx exists bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const; @@ -138,7 +138,6 @@ using OcTreed = OcTree; /// @brief compute the bounding volume of an octree node's i-th child template -FCL_EXPORT void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv); } // namespace fcl diff --git a/include/fcl/geometry/shape/box-inl.h b/include/fcl/geometry/shape/box-inl.h index 053119a0e..b991852cb 100644 --- a/include/fcl/geometry/shape/box-inl.h +++ b/include/fcl/geometry/shape/box-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Box; +class FCL_EXTERN_TEMPLATE_API Box; //============================================================================== template diff --git a/include/fcl/geometry/shape/box.h b/include/fcl/geometry/shape/box.h index d15e9f269..5460828a1 100644 --- a/include/fcl/geometry/shape/box.h +++ b/include/fcl/geometry/shape/box.h @@ -47,7 +47,7 @@ namespace fcl /// @brief Center at zero point, axis aligned box template -class FCL_EXPORT Box : public ShapeBase +class Box : public ShapeBase { public: diff --git a/include/fcl/geometry/shape/capsule-inl.h b/include/fcl/geometry/shape/capsule-inl.h index b2add91fc..a2e0b8a3a 100644 --- a/include/fcl/geometry/shape/capsule-inl.h +++ b/include/fcl/geometry/shape/capsule-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Capsule; +class FCL_EXTERN_TEMPLATE_API Capsule; //============================================================================== template diff --git a/include/fcl/geometry/shape/capsule.h b/include/fcl/geometry/shape/capsule.h index 5add95e27..8c21a96fe 100644 --- a/include/fcl/geometry/shape/capsule.h +++ b/include/fcl/geometry/shape/capsule.h @@ -45,9 +45,9 @@ namespace fcl { -/// @brief Center at zero point capsule +/// @brief Center at zero point capsule template -class FCL_EXPORT Capsule : public ShapeBase +class Capsule : public ShapeBase { public: @@ -56,16 +56,16 @@ class FCL_EXPORT Capsule : public ShapeBase /// @brief Constructor Capsule(S radius, S lz); - /// @brief Radius of capsule + /// @brief Radius of capsule S radius; - /// @brief Length along z axis + /// @brief Length along z axis S lz; /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a capsule + /// @brief Get node type: a capsule NODE_TYPE getNodeType() const override; // Documentation inherited diff --git a/include/fcl/geometry/shape/cone-inl.h b/include/fcl/geometry/shape/cone-inl.h index 3b023fcc0..08766603f 100644 --- a/include/fcl/geometry/shape/cone-inl.h +++ b/include/fcl/geometry/shape/cone-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Cone; +class FCL_EXTERN_TEMPLATE_API Cone; //============================================================================== template diff --git a/include/fcl/geometry/shape/cone.h b/include/fcl/geometry/shape/cone.h index ef9180c2a..30fff4305 100644 --- a/include/fcl/geometry/shape/cone.h +++ b/include/fcl/geometry/shape/cone.h @@ -45,9 +45,9 @@ namespace fcl { -/// @brief Center at zero cone +/// @brief Center at zero cone template -class FCL_EXPORT Cone : public ShapeBase +class Cone : public ShapeBase { public: @@ -55,16 +55,16 @@ class FCL_EXPORT Cone : public ShapeBase Cone(S radius, S lz); - /// @brief Radius of the cone + /// @brief Radius of the cone S radius; - /// @brief Length along z axis + /// @brief Length along z axis S lz; /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a cone + /// @brief Get node type: a cone NODE_TYPE getNodeType() const override; // Documentation inherited diff --git a/include/fcl/geometry/shape/convex-inl.h b/include/fcl/geometry/shape/convex-inl.h index 597545e44..8e29b595c 100644 --- a/include/fcl/geometry/shape/convex-inl.h +++ b/include/fcl/geometry/shape/convex-inl.h @@ -46,7 +46,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Convex; +class FCL_EXTERN_TEMPLATE_API Convex; //============================================================================== template diff --git a/include/fcl/geometry/shape/convex.h b/include/fcl/geometry/shape/convex.h index 03c0489a5..8a47b67f0 100644 --- a/include/fcl/geometry/shape/convex.h +++ b/include/fcl/geometry/shape/convex.h @@ -78,7 +78,7 @@ namespace fcl /// /// @tparam S_ The scalar type; must be a valid Eigen scalar. template -class FCL_EXPORT Convex : public ShapeBase +class Convex : public ShapeBase { public: @@ -103,7 +103,7 @@ class FCL_EXPORT Convex : public ShapeBase Convex(const std::shared_ptr>>& vertices, int num_faces, const std::shared_ptr>& faces); - /// @brief Copy constructor + /// @brief Copy constructor Convex(const Convex& other); ~Convex() = default; diff --git a/include/fcl/geometry/shape/cylinder-inl.h b/include/fcl/geometry/shape/cylinder-inl.h index f5a621153..d019f9501 100644 --- a/include/fcl/geometry/shape/cylinder-inl.h +++ b/include/fcl/geometry/shape/cylinder-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Cylinder; +class FCL_EXTERN_TEMPLATE_API Cylinder; //============================================================================== template diff --git a/include/fcl/geometry/shape/cylinder.h b/include/fcl/geometry/shape/cylinder.h index bd67d3132..6e7280d1e 100644 --- a/include/fcl/geometry/shape/cylinder.h +++ b/include/fcl/geometry/shape/cylinder.h @@ -45,9 +45,9 @@ namespace fcl { -/// @brief Center at zero cylinder +/// @brief Center at zero cylinder template -class FCL_EXPORT Cylinder : public ShapeBase +class Cylinder : public ShapeBase { public: @@ -55,17 +55,17 @@ class FCL_EXPORT Cylinder : public ShapeBase /// @brief Constructor Cylinder(S radius, S lz); - - /// @brief Radius of the cylinder + + /// @brief Radius of the cylinder S radius; - /// @brief Length along z axis + /// @brief Length along z axis S lz; - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a cylinder + /// @brief Get node type: a cylinder NODE_TYPE getNodeType() const override; // Documentation inherited diff --git a/include/fcl/geometry/shape/ellipsoid-inl.h b/include/fcl/geometry/shape/ellipsoid-inl.h index 5bbf946f8..fda3f3f3a 100644 --- a/include/fcl/geometry/shape/ellipsoid-inl.h +++ b/include/fcl/geometry/shape/ellipsoid-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Ellipsoid; +class FCL_EXTERN_TEMPLATE_API Ellipsoid; //============================================================================== template diff --git a/include/fcl/geometry/shape/ellipsoid.h b/include/fcl/geometry/shape/ellipsoid.h index 85632a900..3dfd1c00c 100644 --- a/include/fcl/geometry/shape/ellipsoid.h +++ b/include/fcl/geometry/shape/ellipsoid.h @@ -47,7 +47,7 @@ namespace fcl /// @brief Center at zero point ellipsoid template -class FCL_EXPORT Ellipsoid : public ShapeBase +class Ellipsoid : public ShapeBase { public: diff --git a/include/fcl/geometry/shape/halfspace-inl.h b/include/fcl/geometry/shape/halfspace-inl.h index bc58f80f1..08e66de9b 100644 --- a/include/fcl/geometry/shape/halfspace-inl.h +++ b/include/fcl/geometry/shape/halfspace-inl.h @@ -45,10 +45,11 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Halfspace; +class FCL_EXTERN_TEMPLATE_API Halfspace; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API Halfspace transform(const Halfspace& a, const Transform3& tf); //============================================================================== diff --git a/include/fcl/geometry/shape/halfspace.h b/include/fcl/geometry/shape/halfspace.h index 374b9e264..12c441a05 100644 --- a/include/fcl/geometry/shape/halfspace.h +++ b/include/fcl/geometry/shape/halfspace.h @@ -54,7 +54,7 @@ namespace fcl /// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points /// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space template -class FCL_EXPORT Halfspace : public ShapeBase +class Halfspace : public ShapeBase { public: @@ -77,10 +77,10 @@ class FCL_EXPORT Halfspace : public ShapeBase /// @brief Get node type: a half space NODE_TYPE getNodeType() const override; - + /// @brief Planed normal Vector3 n; - + /// @brief Planed offset S d; @@ -101,7 +101,6 @@ using Halfspacef = Halfspace; using Halfspaced = Halfspace; template -FCL_EXPORT Halfspace transform(const Halfspace& a, const Transform3& tf); } // namespace fcl diff --git a/include/fcl/geometry/shape/plane-inl.h b/include/fcl/geometry/shape/plane-inl.h index 2e5da9c06..ef2bb94b1 100644 --- a/include/fcl/geometry/shape/plane-inl.h +++ b/include/fcl/geometry/shape/plane-inl.h @@ -45,10 +45,11 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Plane; +class FCL_EXTERN_TEMPLATE_API Plane; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API Plane transform(const Plane& a, const Transform3& tf); //============================================================================== diff --git a/include/fcl/geometry/shape/plane.h b/include/fcl/geometry/shape/plane.h index bcf34212e..77a8ce2f7 100644 --- a/include/fcl/geometry/shape/plane.h +++ b/include/fcl/geometry/shape/plane.h @@ -45,18 +45,18 @@ namespace fcl { -/// @brief Infinite plane +/// @brief Infinite plane template -class FCL_EXPORT Plane : public ShapeBase +class Plane : public ShapeBase { public: using S = S_; - /// @brief Construct a plane with normal direction and offset + /// @brief Construct a plane with normal direction and offset Plane(const Vector3& n, S d); - - /// @brief Construct a plane with normal direction and offset + + /// @brief Construct a plane with normal direction and offset Plane(S a, S b, S c, S d); Plane(); @@ -68,13 +68,13 @@ class FCL_EXPORT Plane : public ShapeBase /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a plane + /// @brief Get node type: a plane NODE_TYPE getNodeType() const override; - /// @brief Plane normal + /// @brief Plane normal Vector3 n; - /// @brief Plane offset + /// @brief Plane offset S d; friend @@ -84,8 +84,8 @@ class FCL_EXPORT Plane : public ShapeBase } protected: - - /// @brief Turn non-unit normal into unit + + /// @brief Turn non-unit normal into unit void unitNormalTest(); }; @@ -93,7 +93,6 @@ using Planef = Plane; using Planed = Plane; template -FCL_EXPORT Plane transform(const Plane& a, const Transform3& tf); } // namespace fcl diff --git a/include/fcl/geometry/shape/shape_base-inl.h b/include/fcl/geometry/shape/shape_base-inl.h index b3148c469..a5defdad4 100644 --- a/include/fcl/geometry/shape/shape_base-inl.h +++ b/include/fcl/geometry/shape/shape_base-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT ShapeBase; +class FCL_EXTERN_TEMPLATE_API ShapeBase; //============================================================================== template diff --git a/include/fcl/geometry/shape/shape_base.h b/include/fcl/geometry/shape/shape_base.h index 6acc6da64..1f68aa270 100644 --- a/include/fcl/geometry/shape/shape_base.h +++ b/include/fcl/geometry/shape/shape_base.h @@ -45,7 +45,7 @@ namespace fcl /// @brief Base class for all basic geometric shapes template -class FCL_EXPORT ShapeBase : public CollisionGeometry +class ShapeBase : public CollisionGeometry { public: diff --git a/include/fcl/geometry/shape/sphere-inl.h b/include/fcl/geometry/shape/sphere-inl.h index 0da7eeae9..75487c55f 100644 --- a/include/fcl/geometry/shape/sphere-inl.h +++ b/include/fcl/geometry/shape/sphere-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Sphere; +class FCL_EXTERN_TEMPLATE_API Sphere; //============================================================================== template diff --git a/include/fcl/geometry/shape/sphere.h b/include/fcl/geometry/shape/sphere.h index 48b67927f..d33a3f05c 100644 --- a/include/fcl/geometry/shape/sphere.h +++ b/include/fcl/geometry/shape/sphere.h @@ -47,7 +47,7 @@ namespace fcl /// @brief Center at zero point sphere template -class FCL_EXPORT Sphere : public ShapeBase +class Sphere : public ShapeBase { public: diff --git a/include/fcl/geometry/shape/triangle_p-inl.h b/include/fcl/geometry/shape/triangle_p-inl.h index f48d41bb7..7f9af5d34 100644 --- a/include/fcl/geometry/shape/triangle_p-inl.h +++ b/include/fcl/geometry/shape/triangle_p-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT TriangleP; +class FCL_EXTERN_TEMPLATE_API TriangleP; //============================================================================== template diff --git a/include/fcl/geometry/shape/triangle_p.h b/include/fcl/geometry/shape/triangle_p.h index 7cc18caa0..da3c92a53 100644 --- a/include/fcl/geometry/shape/triangle_p.h +++ b/include/fcl/geometry/shape/triangle_p.h @@ -47,7 +47,7 @@ namespace fcl /// @brief Triangle stores the points instead of only indices of points template -class FCL_EXPORT TriangleP : public ShapeBase +class TriangleP : public ShapeBase { public: @@ -59,7 +59,7 @@ class FCL_EXPORT TriangleP : public ShapeBase /// @brief virtual function of compute AABB in local coordinate void computeLocalAABB() override; - + // Documentation inherited NODE_TYPE getNodeType() const override; diff --git a/include/fcl/geometry/shape/utility-inl.h b/include/fcl/geometry/shape/utility-inl.h index 146819daf..bca2fa957 100644 --- a/include/fcl/geometry/shape/utility-inl.h +++ b/include/fcl/geometry/shape/utility-inl.h @@ -58,62 +58,77 @@ namespace fcl { //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const OBB& bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const OBBRSS& bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const kIOS& bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const RSS& bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const AABB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const OBB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const OBBRSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const kIOS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const RSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== @@ -122,7 +137,7 @@ namespace detail { //============================================================================== template -struct FCL_EXPORT ComputeBVImpl +struct ComputeBVImpl { static void run(const Shape& s, const Transform3& tf, BV& bv) { @@ -134,7 +149,7 @@ struct FCL_EXPORT ComputeBVImpl //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Box> +struct ComputeBVImpl, Box> { static void run(const Box& s, const Transform3& tf, AABB& bv) { @@ -153,7 +168,7 @@ struct FCL_EXPORT ComputeBVImpl, Box> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Box> +struct ComputeBVImpl, Box> { static void run(const Box& s, const Transform3& tf, OBB& bv) { @@ -165,7 +180,7 @@ struct FCL_EXPORT ComputeBVImpl, Box> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Capsule> +struct ComputeBVImpl, Capsule> { static void run(const Capsule& s, const Transform3& tf, AABB& bv) { @@ -184,7 +199,7 @@ struct FCL_EXPORT ComputeBVImpl, Capsule> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Capsule> +struct ComputeBVImpl, Capsule> { static void run(const Capsule& s, const Transform3& tf, OBB& bv) { @@ -196,7 +211,7 @@ struct FCL_EXPORT ComputeBVImpl, Capsule> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Cone> +struct ComputeBVImpl, Cone> { static void run(const Cone& s, const Transform3& tf, AABB& bv) { @@ -215,7 +230,7 @@ struct FCL_EXPORT ComputeBVImpl, Cone> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Cone> +struct ComputeBVImpl, Cone> { static void run(const Cone& s, const Transform3& tf, OBB& bv) { @@ -227,7 +242,7 @@ struct FCL_EXPORT ComputeBVImpl, Cone> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Convex> +struct ComputeBVImpl, Convex> { static void run(const Convex& s, const Transform3& tf, AABB& bv) { @@ -247,7 +262,7 @@ struct FCL_EXPORT ComputeBVImpl, Convex> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Convex> +struct ComputeBVImpl, Convex> { static void run(const Convex& s, const Transform3& tf, OBB& bv) { @@ -260,7 +275,7 @@ struct FCL_EXPORT ComputeBVImpl, Convex> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Cylinder> +struct ComputeBVImpl, Cylinder> { static void run(const Cylinder& s, const Transform3& tf, AABB& bv) { @@ -279,7 +294,7 @@ struct FCL_EXPORT ComputeBVImpl, Cylinder> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Cylinder> +struct ComputeBVImpl, Cylinder> { static void run(const Cylinder& s, const Transform3& tf, OBB& bv) { @@ -291,7 +306,7 @@ struct FCL_EXPORT ComputeBVImpl, Cylinder> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Ellipsoid> +struct ComputeBVImpl, Ellipsoid> { static void run(const Ellipsoid& s, const Transform3& tf, AABB& bv) { @@ -310,7 +325,7 @@ struct FCL_EXPORT ComputeBVImpl, Ellipsoid> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Ellipsoid> +struct ComputeBVImpl, Ellipsoid> { static void run(const Ellipsoid& s, const Transform3& tf, OBB& bv) { @@ -322,7 +337,7 @@ struct FCL_EXPORT ComputeBVImpl, Ellipsoid> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Halfspace> +struct ComputeBVImpl, Halfspace> { static void run(const Halfspace& s, const Transform3& tf, AABB& bv) { @@ -358,7 +373,7 @@ struct FCL_EXPORT ComputeBVImpl, Halfspace> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Halfspace> +struct ComputeBVImpl, Halfspace> { static void run(const Halfspace& s, const Transform3& tf, OBB& bv) { @@ -374,7 +389,7 @@ struct FCL_EXPORT ComputeBVImpl, Halfspace> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Halfspace> +struct ComputeBVImpl, Halfspace> { static void run(const Halfspace& s, const Transform3& tf, RSS& bv) { @@ -390,7 +405,7 @@ struct FCL_EXPORT ComputeBVImpl, Halfspace> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Halfspace> +struct ComputeBVImpl, Halfspace> { static void run(const Halfspace& s, const Transform3& tf, OBBRSS& bv) { @@ -401,7 +416,7 @@ struct FCL_EXPORT ComputeBVImpl, Halfspace> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Halfspace> +struct ComputeBVImpl, Halfspace> { static void run(const Halfspace& s, const Transform3& tf, kIOS& bv) { @@ -414,7 +429,7 @@ struct FCL_EXPORT ComputeBVImpl, Halfspace> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Halfspace> +struct ComputeBVImpl, Halfspace> { static void run(const Halfspace& s, const Transform3& tf, KDOP& bv) { @@ -473,7 +488,7 @@ struct FCL_EXPORT ComputeBVImpl, Halfspace> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Halfspace> +struct ComputeBVImpl, Halfspace> { static void run(const Halfspace& s, const Transform3& tf, KDOP& bv) { @@ -538,7 +553,7 @@ struct FCL_EXPORT ComputeBVImpl, Halfspace> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Halfspace> +struct ComputeBVImpl, Halfspace> { static void run(const Halfspace& s, const Transform3& tf, KDOP& bv) { @@ -618,7 +633,7 @@ struct FCL_EXPORT ComputeBVImpl, Halfspace> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Plane> +struct ComputeBVImpl, Plane> { static void run(const Plane& s, const Transform3& tf, AABB& bv) { @@ -654,7 +669,7 @@ struct FCL_EXPORT ComputeBVImpl, Plane> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Plane> +struct ComputeBVImpl, Plane> { static void run(const Plane& s, const Transform3& tf, OBB& bv) { @@ -671,7 +686,7 @@ struct FCL_EXPORT ComputeBVImpl, Plane> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Plane> +struct ComputeBVImpl, Plane> { static void run(const Plane& s, const Transform3& tf, RSS& bv) { @@ -692,7 +707,7 @@ struct FCL_EXPORT ComputeBVImpl, Plane> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Plane> +struct ComputeBVImpl, Plane> { static void run(const Plane& s, const Transform3& tf, OBBRSS& bv) { @@ -703,7 +718,7 @@ struct FCL_EXPORT ComputeBVImpl, Plane> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Plane> +struct ComputeBVImpl, Plane> { static void run(const Plane& s, const Transform3& tf, kIOS& bv) { @@ -716,7 +731,7 @@ struct FCL_EXPORT ComputeBVImpl, Plane> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Plane> +struct ComputeBVImpl, Plane> { static void run(const Plane& s, const Transform3& tf, KDOP& bv) { @@ -771,7 +786,7 @@ struct FCL_EXPORT ComputeBVImpl, Plane> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Plane> +struct ComputeBVImpl, Plane> { static void run(const Plane& s, const Transform3& tf, KDOP& bv) { @@ -830,7 +845,7 @@ struct FCL_EXPORT ComputeBVImpl, Plane> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Plane> +struct ComputeBVImpl, Plane> { static void run(const Plane& s, const Transform3& tf, KDOP& bv) { @@ -901,7 +916,7 @@ struct FCL_EXPORT ComputeBVImpl, Plane> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Sphere> +struct ComputeBVImpl, Sphere> { static void run(const Sphere& s, const Transform3& tf, AABB& bv) { @@ -913,7 +928,7 @@ struct FCL_EXPORT ComputeBVImpl, Sphere> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, Sphere> +struct ComputeBVImpl, Sphere> { static void run(const Sphere& s, const Transform3& tf, OBB& bv) { @@ -925,7 +940,7 @@ struct FCL_EXPORT ComputeBVImpl, Sphere> //============================================================================== template -struct FCL_EXPORT ComputeBVImpl, TriangleP> +struct ComputeBVImpl, TriangleP> { static void run(const TriangleP& s, const Transform3& tf, AABB& bv) { @@ -935,119 +950,119 @@ struct FCL_EXPORT ComputeBVImpl, TriangleP> //============================================================================== extern template -struct ComputeBVImpl, Box>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Box>; //============================================================================== extern template -struct ComputeBVImpl, Box>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Box>; //============================================================================== extern template -struct ComputeBVImpl, Capsule>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Capsule>; //============================================================================== extern template -struct ComputeBVImpl, Capsule>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Capsule>; //============================================================================== extern template -struct ComputeBVImpl, Cone>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Cone>; //============================================================================== extern template -struct ComputeBVImpl, Cone>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Cone>; //============================================================================== extern template -struct ComputeBVImpl, Cylinder>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Cylinder>; //============================================================================== extern template -struct ComputeBVImpl, Cylinder>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Cylinder>; //============================================================================== extern template -struct ComputeBVImpl, Ellipsoid>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Ellipsoid>; //============================================================================== extern template -struct ComputeBVImpl, Ellipsoid>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Ellipsoid>; //============================================================================== extern template -struct ComputeBVImpl, Halfspace>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct ComputeBVImpl, Halfspace>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct ComputeBVImpl, Halfspace>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct ComputeBVImpl, Halfspace>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct ComputeBVImpl, Halfspace>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct ComputeBVImpl, Halfspace>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct ComputeBVImpl, Halfspace>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct ComputeBVImpl, Halfspace>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct ComputeBVImpl, Plane>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; //============================================================================== extern template -struct ComputeBVImpl, Plane>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; //============================================================================== extern template -struct ComputeBVImpl, Plane>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; //============================================================================== extern template -struct ComputeBVImpl, Plane>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; //============================================================================== extern template -struct ComputeBVImpl, Plane>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; //============================================================================== extern template -struct ComputeBVImpl, Plane>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; //============================================================================== extern template -struct ComputeBVImpl, Plane>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; //============================================================================== extern template -struct ComputeBVImpl, Plane>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; //============================================================================== extern template -struct ComputeBVImpl, Sphere>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Sphere>; //============================================================================== extern template -struct ComputeBVImpl, Sphere>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Sphere>; //============================================================================== extern template -struct ComputeBVImpl, TriangleP>; +struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, TriangleP>; //============================================================================== } // namespace detail @@ -1055,7 +1070,6 @@ struct ComputeBVImpl, TriangleP>; //============================================================================== template -FCL_EXPORT void computeBV(const Shape& s, const Transform3& tf, BV& bv) { using S = typename BV::S; diff --git a/include/fcl/geometry/shape/utility.h b/include/fcl/geometry/shape/utility.h index ec70def9a..fe0bc62fc 100644 --- a/include/fcl/geometry/shape/utility.h +++ b/include/fcl/geometry/shape/utility.h @@ -57,72 +57,55 @@ namespace fcl /// @brief calculate a bounding volume for a shape in a specific configuration template -FCL_EXPORT void computeBV(const Shape& s, const Transform3& tf, BV& bv); /// @brief construct a box shape (with a configuration) from a given bounding volume template -FCL_EXPORT void constructBox(const AABB& bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const OBB& bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const OBBRSS& bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const kIOS& bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const RSS& bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const KDOP& bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const KDOP& bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const KDOP& bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const AABB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const OBB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const OBBRSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const kIOS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const RSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); template -FCL_EXPORT void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); } // namespace fcl diff --git a/include/fcl/math/bv/AABB-inl.h b/include/fcl/math/bv/AABB-inl.h index 67380068f..5b59b15f2 100644 --- a/include/fcl/math/bv/AABB-inl.h +++ b/include/fcl/math/bv/AABB-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT AABB; +class FCL_EXTERN_TEMPLATE_API AABB; //============================================================================== template diff --git a/include/fcl/math/bv/AABB.h b/include/fcl/math/bv/AABB.h index f84b737ee..d785a6320 100644 --- a/include/fcl/math/bv/AABB.h +++ b/include/fcl/math/bv/AABB.h @@ -46,7 +46,7 @@ namespace fcl /// @brief A class describing the AABB collision structure, which is a box in 3D /// space determined by two diagonal points template -class FCL_EXPORT AABB +class AABB { public: diff --git a/include/fcl/math/bv/OBB-inl.h b/include/fcl/math/bv/OBB-inl.h index 88813229a..51047884f 100644 --- a/include/fcl/math/bv/OBB-inl.h +++ b/include/fcl/math/bv/OBB-inl.h @@ -47,22 +47,26 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT OBB; +class FCL_EXTERN_TEMPLATE_API OBB; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void computeVertices(const OBB& b, Vector3 vertices[8]); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API OBB merge_largedist(const OBB& b1, const OBB& b2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API OBB merge_smalldist(const OBB& b1, const OBB& b2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool obbDisjoint( const Matrix3& B, const Vector3& T, @@ -71,6 +75,7 @@ bool obbDisjoint( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool obbDisjoint( const Transform3& tf, const Vector3& a, diff --git a/include/fcl/math/bv/OBB.h b/include/fcl/math/bv/OBB.h index 3d7e216ba..1f3f0c4ef 100644 --- a/include/fcl/math/bv/OBB.h +++ b/include/fcl/math/bv/OBB.h @@ -48,7 +48,7 @@ namespace fcl /// @brief Oriented bounding box class template -class FCL_EXPORT OBB +class OBB { public: @@ -75,10 +75,10 @@ class FCL_EXPORT OBB const Vector3& center, const Vector3& extent); - /// @brief Check collision between two OBB, return true if collision happens. + /// @brief Check collision between two OBB, return true if collision happens. bool overlap(const OBB& other) const; - - /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. + + /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. bool overlap(const OBB& other, OBB& overlap_part) const; /// @brief Check whether the OBB contains a point. @@ -124,29 +124,24 @@ using OBBd = OBB; /// @brief Compute the 8 vertices of a OBB template -FCL_EXPORT void computeVertices(const OBB& b, Vector3 vertices[8]); /// @brief OBB merge method when the centers of two smaller OBB are far away template -FCL_EXPORT OBB merge_largedist(const OBB& b1, const OBB& b2); /// @brief OBB merge method when the centers of two smaller OBB are close template -FCL_EXPORT OBB merge_smalldist(const OBB& b1, const OBB& b2); /// @brief Translate the OBB bv template -FCL_EXPORT OBB translate( const OBB& bv, const Eigen::MatrixBase& t); /// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and /// b2 is in identity. template -FCL_EXPORT bool overlap(const Eigen::MatrixBase& R0, const Eigen::MatrixBase& T0, const OBB& b1, const OBB& b2); @@ -155,7 +150,6 @@ bool overlap(const Eigen::MatrixBase& R0, /// (R, T) and its half dimension is set by a; the second box is in identity /// configuration and its half dimension is set by b. template -FCL_EXPORT bool obbDisjoint( const Matrix3& B, const Vector3& T, @@ -166,7 +160,6 @@ bool obbDisjoint( /// (R, T) and its half dimension is set by a; the second box is in identity /// configuration and its half dimension is set by b. template -FCL_EXPORT bool obbDisjoint( const Transform3& tf, const Vector3& a, diff --git a/include/fcl/math/bv/OBBRSS-inl.h b/include/fcl/math/bv/OBBRSS-inl.h index 814a66f62..fbfe042b0 100644 --- a/include/fcl/math/bv/OBBRSS-inl.h +++ b/include/fcl/math/bv/OBBRSS-inl.h @@ -45,10 +45,11 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT OBBRSS; +class FCL_EXTERN_TEMPLATE_API OBBRSS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API OBBRSS translate(const OBBRSS& bv, const Vector3& t); //============================================================================== diff --git a/include/fcl/math/bv/OBBRSS.h b/include/fcl/math/bv/OBBRSS.h index 9f84c1105..4f1d65623 100644 --- a/include/fcl/math/bv/OBBRSS.h +++ b/include/fcl/math/bv/OBBRSS.h @@ -47,7 +47,7 @@ namespace fcl /// @brief Class merging the OBB and RSS, can handle collision and distance /// simultaneously template -class FCL_EXPORT OBBRSS +class OBBRSS { public: @@ -108,13 +108,11 @@ using OBBRSSd = OBBRSS; /// @brief Translate the OBBRSS bv template -FCL_EXPORT OBBRSS translate(const OBBRSS& bv, const Vector3& t); /// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) /// and b2 is in indentity template -FCL_EXPORT bool overlap(const Eigen::MatrixBase& R0, const Eigen::MatrixBase& T0, const OBBRSS& b1, const OBBRSS& b2); @@ -122,7 +120,6 @@ bool overlap(const Eigen::MatrixBase& R0, /// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) /// and b2 is in indentity template -FCL_EXPORT bool overlap( const Transform3& tf, const OBBRSS& b1, @@ -131,7 +128,6 @@ bool overlap( /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) /// and b2 is in indentity; P and Q, is not nullptr, returns the nearest points template -FCL_EXPORT S distance( const Eigen::MatrixBase& R0, const Eigen::MatrixBase& T0, @@ -141,7 +137,6 @@ S distance( /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) /// and b2 is in indentity; P and Q, is not nullptr, returns the nearest points template -FCL_EXPORT S distance( const Transform3& tf, const OBBRSS& b1, diff --git a/include/fcl/math/bv/RSS-inl.h b/include/fcl/math/bv/RSS-inl.h index a88d5cb20..bf41c8f0a 100644 --- a/include/fcl/math/bv/RSS-inl.h +++ b/include/fcl/math/bv/RSS-inl.h @@ -45,14 +45,16 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT RSS; +class FCL_EXTERN_TEMPLATE_API RSS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void clipToRange(double& val, double a, double b); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void segCoords( double& t, double& u, @@ -64,6 +66,7 @@ void segCoords( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool inVoronoi( double a, double b, @@ -75,6 +78,7 @@ bool inVoronoi( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double rectDistance( const Matrix3& Rab, const Vector3& Tab, @@ -85,6 +89,7 @@ double rectDistance( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double rectDistance( const Transform3& tfab, const double a[2], @@ -94,6 +99,7 @@ double rectDistance( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API RSS translate(const RSS& bv, const Vector3& t); //============================================================================== diff --git a/include/fcl/math/bv/RSS.h b/include/fcl/math/bv/RSS.h index 125ca3d65..bdb2d07c1 100644 --- a/include/fcl/math/bv/RSS.h +++ b/include/fcl/math/bv/RSS.h @@ -46,7 +46,7 @@ namespace fcl /// @brief A class for rectangle sphere-swept bounding volume template -class FCL_EXPORT RSS +class RSS { public: @@ -123,7 +123,6 @@ using RSSd = RSS; /// @brief Clip value between a and b template -FCL_EXPORT void clipToRange(S& val, S a, S b); /// @brief Finds the parameters t & u corresponding to the two closest points on @@ -139,7 +138,6 @@ void clipToRange(S& val, S a, S b); /// Reference: "On fast computation of distance between line segments." Vladimir /// J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985. template -FCL_EXPORT void segCoords(S& t, S& u, S a, S b, S A_dot_B, S A_dot_T, S B_dot_T); @@ -149,7 +147,6 @@ void segCoords(S& t, S& u, S a, S b, /// determined by the point Pa and the direction Anorm. /// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb. template -FCL_EXPORT bool inVoronoi(S a, S b, S Anorm_dot_B, S Anorm_dot_T, S A_dot_B, S A_dot_T, S B_dot_T); @@ -158,7 +155,6 @@ bool inVoronoi(S a, S b, /// values) are the closest points in the rectangles, both are in the local /// frame of the first rectangle. template -FCL_EXPORT S rectDistance( const Matrix3& Rab, const Vector3& Tab, @@ -171,7 +167,6 @@ S rectDistance( /// values) are the closest points in the rectangles, both are in the local /// frame of the first rectangle. template -FCL_EXPORT S rectDistance( const Transform3& tfab, const S a[2], @@ -185,7 +180,6 @@ S rectDistance( /// points. Notice that P and Q are both in the local frame of the first RSS /// (not global frame and not even the local frame of object 1) template -FCL_EXPORT S distance( const Eigen::MatrixBase& R0, const Eigen::MatrixBase& T0, @@ -197,7 +191,6 @@ S distance( /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and /// b2 is in identity. template -FCL_EXPORT bool overlap( const Eigen::MatrixBase& R0, const Eigen::MatrixBase& T0, @@ -206,7 +199,6 @@ bool overlap( /// @brief Translate the RSS bv template -FCL_EXPORT RSS translate(const RSS& bv, const Vector3& t); } // namespace fcl diff --git a/include/fcl/math/bv/kDOP-inl.h b/include/fcl/math/bv/kDOP-inl.h index 371fdb61a..76fd22151 100644 --- a/include/fcl/math/bv/kDOP-inl.h +++ b/include/fcl/math/bv/kDOP-inl.h @@ -47,39 +47,43 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT KDOP; +class FCL_EXTERN_TEMPLATE_API KDOP; //============================================================================== extern template -class FCL_EXPORT KDOP; +class FCL_EXTERN_TEMPLATE_API KDOP; //============================================================================== extern template -class FCL_EXPORT KDOP; +class FCL_EXTERN_TEMPLATE_API KDOP; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void minmax(double a, double b, double& minv, double& maxv); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void minmax(double p, double& minv, double& maxv); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void getDistances(const Vector3& p, double* d); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void getDistances(const Vector3& p, double* d); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void getDistances(const Vector3& p, double* d); //============================================================================== template -FCL_EXPORT KDOP::KDOP() { static_assert(N == 16 || N == 18 || N == 24, "N should be 16, 18, or 24"); @@ -94,7 +98,6 @@ KDOP::KDOP() //============================================================================== template -FCL_EXPORT KDOP::KDOP(const Vector3& v) { for(std::size_t i = 0; i < 3; ++i) @@ -112,7 +115,6 @@ KDOP::KDOP(const Vector3& v) //============================================================================== template -FCL_EXPORT KDOP::KDOP(const Vector3& a, const Vector3& b) { for(std::size_t i = 0; i < 3; ++i) @@ -131,7 +133,6 @@ KDOP::KDOP(const Vector3& a, const Vector3& b) //============================================================================== template -FCL_EXPORT bool KDOP::overlap(const KDOP& other) const { for(std::size_t i = 0; i < N / 2; ++i) @@ -145,7 +146,6 @@ bool KDOP::overlap(const KDOP& other) const //============================================================================== template -FCL_EXPORT bool KDOP::inside(const Vector3& p) const { for(std::size_t i = 0; i < 3; ++i) @@ -167,7 +167,6 @@ bool KDOP::inside(const Vector3& p) const //============================================================================== template -FCL_EXPORT KDOP& KDOP::operator += (const Vector3& p) { for(std::size_t i = 0; i < 3; ++i) @@ -187,7 +186,6 @@ KDOP& KDOP::operator += (const Vector3& p) //============================================================================== template -FCL_EXPORT KDOP& KDOP::operator += (const KDOP& other) { for(std::size_t i = 0; i < N / 2; ++i) @@ -200,7 +198,6 @@ KDOP& KDOP::operator += (const KDOP& other) //============================================================================== template -FCL_EXPORT KDOP KDOP::operator + (const KDOP& other) const { KDOP res(*this); @@ -209,7 +206,6 @@ KDOP KDOP::operator + (const KDOP& other) const //============================================================================== template -FCL_EXPORT S KDOP::width() const { return dist_[N / 2] - dist_[0]; @@ -217,7 +213,6 @@ S KDOP::width() const //============================================================================== template -FCL_EXPORT S KDOP::height() const { return dist_[N / 2 + 1] - dist_[1]; @@ -225,7 +220,6 @@ S KDOP::height() const //============================================================================== template -FCL_EXPORT S KDOP::depth() const { return dist_[N / 2 + 2] - dist_[2]; @@ -233,7 +227,6 @@ S KDOP::depth() const //============================================================================== template -FCL_EXPORT S KDOP::volume() const { return width() * height() * depth(); @@ -241,7 +234,6 @@ S KDOP::volume() const //============================================================================== template -FCL_EXPORT S KDOP::size() const { return width() * width() + height() * height() + depth() * depth(); @@ -249,7 +241,6 @@ S KDOP::size() const //============================================================================== template -FCL_EXPORT Vector3 KDOP::center() const { return Vector3(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5; @@ -257,7 +248,6 @@ Vector3 KDOP::center() const //============================================================================== template -FCL_EXPORT S KDOP::distance(const KDOP& other, Vector3* P, Vector3* Q) const { FCL_UNUSED(other); @@ -270,7 +260,6 @@ S KDOP::distance(const KDOP& other, Vector3* P, Vector3* Q) co //============================================================================== template -FCL_EXPORT S KDOP::dist(std::size_t i) const { return dist_[i]; @@ -278,7 +267,6 @@ S KDOP::dist(std::size_t i) const //============================================================================== template -FCL_EXPORT S& KDOP::dist(std::size_t i) { return dist_[i]; @@ -286,7 +274,6 @@ S& KDOP::dist(std::size_t i) //============================================================================== template -FCL_EXPORT KDOP translate( const KDOP& bv, const Eigen::MatrixBase& t) { @@ -310,7 +297,6 @@ KDOP translate( //============================================================================== template -FCL_EXPORT void minmax(S a, S b, S& minv, S& maxv) { if(a > b) @@ -327,7 +313,6 @@ void minmax(S a, S b, S& minv, S& maxv) //============================================================================== template -FCL_EXPORT void minmax(S p, S& minv, S& maxv) { if(p > maxv) maxv = p; @@ -346,7 +331,6 @@ struct GetDistancesImpl //============================================================================== template -FCL_EXPORT void getDistances(const Vector3& p, S* d) { GetDistancesImpl::run(p, d); @@ -354,7 +338,7 @@ void getDistances(const Vector3& p, S* d) //============================================================================== template -struct FCL_EXPORT GetDistancesImpl +struct GetDistancesImpl { static void run(const Vector3& p, S* d) { @@ -368,7 +352,7 @@ struct FCL_EXPORT GetDistancesImpl //============================================================================== template -struct FCL_EXPORT GetDistancesImpl +struct GetDistancesImpl { static void run(const Vector3& p, S* d) { @@ -383,7 +367,7 @@ struct FCL_EXPORT GetDistancesImpl //============================================================================== template -struct FCL_EXPORT GetDistancesImpl +struct GetDistancesImpl { static void run(const Vector3& p, S* d) { diff --git a/include/fcl/math/bv/kDOP.h b/include/fcl/math/bv/kDOP.h index 54b7e9c12..31dc4ba74 100644 --- a/include/fcl/math/bv/kDOP.h +++ b/include/fcl/math/bv/kDOP.h @@ -47,7 +47,7 @@ namespace fcl { /// @brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 -/// The KDOP structure is defined by some pairs of parallel planes defined by some axes. +/// The KDOP structure is defined by some pairs of parallel planes defined by some axes. /// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 8 /// (0,-1,0) and (0,1,0) -> indices 1 and 9 @@ -81,7 +81,7 @@ namespace fcl /// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22 /// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23 template -class FCL_EXPORT KDOP +class KDOP { public: @@ -95,7 +95,7 @@ class FCL_EXPORT KDOP /// @brief Creating kDOP containing two points KDOP(const Vector3& a, const Vector3& b); - + /// @brief Check whether two KDOPs are overlapped bool overlap(const KDOP& other) const; @@ -152,23 +152,19 @@ using KDOPd = KDOP; /// @brief Find the smaller and larger one of two values template -FCL_EXPORT void minmax(S a, S b, S& minv, S& maxv); /// @brief Merge the interval [minv, maxv] and value p/ template -FCL_EXPORT void minmax(S p, S& minv, S& maxv); /// @brief Compute the distances to planes with normals from KDOP vectors except /// those of AABB face planes template -FCL_EXPORT void getDistances(const Vector3& p, S* d); /// @brief translate the KDOP BV template -FCL_EXPORT KDOP translate( const KDOP& bv, const Eigen::MatrixBase& t); diff --git a/include/fcl/math/bv/kIOS-inl.h b/include/fcl/math/bv/kIOS-inl.h index 81a802a8c..285048d18 100644 --- a/include/fcl/math/bv/kIOS-inl.h +++ b/include/fcl/math/bv/kIOS-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT kIOS; +class FCL_EXTERN_TEMPLATE_API kIOS; //============================================================================== template diff --git a/include/fcl/math/bv/kIOS.h b/include/fcl/math/bv/kIOS.h index 85f8933af..0ce7ba496 100644 --- a/include/fcl/math/bv/kIOS.h +++ b/include/fcl/math/bv/kIOS.h @@ -42,10 +42,10 @@ namespace fcl { - + /// @brief A class describing the kIOS collision structure, which is a set of spheres. template -class FCL_EXPORT kIOS +class kIOS { /// @brief One sphere in kIOS struct kIOS_Sphere @@ -126,7 +126,6 @@ using kIOSd = kIOS; /// @brief Translate the kIOS BV template -FCL_EXPORT kIOS translate( const kIOS& bv, const Eigen::MatrixBase& t); @@ -134,7 +133,6 @@ kIOS translate( /// and b2 is in identity. /// @todo Not efficient template -FCL_EXPORT bool overlap( const Eigen::MatrixBase& R0, const Eigen::MatrixBase& T0, @@ -144,7 +142,6 @@ bool overlap( /// and b2 is in identity. /// @todo Not efficient template -FCL_EXPORT bool overlap( const Transform3& tf, const kIOS& b1, @@ -153,7 +150,6 @@ bool overlap( /// @brief Approximate distance between two kIOS bounding volumes /// @todo P and Q is not returned, need implementation template -FCL_EXPORT S distance( const Eigen::MatrixBase& R0, const Eigen::MatrixBase& T0, @@ -165,7 +161,6 @@ S distance( /// @brief Approximate distance between two kIOS bounding volumes /// @todo P and Q is not returned, need implementation template -FCL_EXPORT S distance( const Transform3& tf, const kIOS& b1, diff --git a/include/fcl/math/bv/utility-inl.h b/include/fcl/math/bv/utility-inl.h index eb8653c1b..83036df26 100644 --- a/include/fcl/math/bv/utility-inl.h +++ b/include/fcl/math/bv/utility-inl.h @@ -62,7 +62,6 @@ namespace OBB_fit_functions { //============================================================================== template -FCL_EXPORT void fit1(const Vector3* const ps, OBB& bv) { bv.To = ps[0]; @@ -72,7 +71,6 @@ void fit1(const Vector3* const ps, OBB& bv) //============================================================================== template -FCL_EXPORT void fit2(const Vector3* const ps, OBB& bv) { const Vector3& p1 = ps[0]; @@ -90,7 +88,6 @@ void fit2(const Vector3* const ps, OBB& bv) //============================================================================== template -FCL_EXPORT void fit3(const Vector3* const ps, OBB& bv) { const Vector3& p1 = ps[0]; @@ -120,7 +117,6 @@ void fit3(const Vector3* const ps, OBB& bv) //============================================================================== template -FCL_EXPORT void fit6(const Vector3* const ps, OBB& bv) { OBB bv1, bv2; @@ -131,7 +127,6 @@ void fit6(const Vector3* const ps, OBB& bv) //============================================================================== template -FCL_EXPORT void fitn(const Vector3* const ps, int n, OBB& bv) { Matrix3 M; @@ -148,22 +143,27 @@ void fitn(const Vector3* const ps, int n, OBB& bv) //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void fit1(const Vector3d* const ps, OBB& bv); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void fit2(const Vector3d* const ps, OBB& bv); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void fit3(const Vector3d* const ps, OBB& bv); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void fit6(const Vector3d* const ps, OBB& bv); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void fitn(const Vector3d* const ps, int n, OBB& bv); //============================================================================== @@ -176,7 +176,6 @@ namespace RSS_fit_functions { //============================================================================== template -FCL_EXPORT void fit1(const Vector3* const ps, RSS& bv) { bv.To = ps[0]; @@ -188,7 +187,6 @@ void fit1(const Vector3* const ps, RSS& bv) //============================================================================== template -FCL_EXPORT void fit2(const Vector3* const ps, RSS& bv) { const Vector3& p1 = ps[0]; @@ -208,7 +206,6 @@ void fit2(const Vector3* const ps, RSS& bv) //============================================================================== template -FCL_EXPORT void fit3(const Vector3* const ps, RSS& bv) { const Vector3& p1 = ps[0]; @@ -236,7 +233,6 @@ void fit3(const Vector3* const ps, RSS& bv) //============================================================================== template -FCL_EXPORT void fit6(const Vector3* const ps, RSS& bv) { RSS bv1, bv2; @@ -247,7 +243,6 @@ void fit6(const Vector3* const ps, RSS& bv) //============================================================================== template -FCL_EXPORT void fitn(const Vector3* const ps, int n, RSS& bv) { Matrix3 M; // row first matrix @@ -264,27 +259,27 @@ void fitn(const Vector3* const ps, int n, RSS& bv) //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API void fit1(const Vector3d* const ps, RSS& bv); //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API void fit2(const Vector3d* const ps, RSS& bv); //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API void fit3(const Vector3d* const ps, RSS& bv); //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API void fit6(const Vector3d* const ps, RSS& bv); //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API void fitn(const Vector3d* const ps, int n, RSS& bv); //============================================================================== @@ -297,7 +292,6 @@ namespace kIOS_fit_functions { //============================================================================== template -FCL_EXPORT void fit1(const Vector3* const ps, kIOS& bv) { bv.num_spheres = 1; @@ -311,7 +305,6 @@ void fit1(const Vector3* const ps, kIOS& bv) //============================================================================== template -FCL_EXPORT void fit2(const Vector3* const ps, kIOS& bv) { bv.num_spheres = 5; @@ -349,7 +342,6 @@ void fit2(const Vector3* const ps, kIOS& bv) //============================================================================== template -FCL_EXPORT void fit3(const Vector3* const ps, kIOS& bv) { bv.num_spheres = 3; @@ -395,7 +387,6 @@ void fit3(const Vector3* const ps, kIOS& bv) //============================================================================== template -FCL_EXPORT void fitn(const Vector3* const ps, int n, kIOS& bv) { Matrix3 M; @@ -463,22 +454,22 @@ void fitn(const Vector3* const ps, int n, kIOS& bv) //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API void fit1(const Vector3d* const ps, kIOS& bv); //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API void fit2(const Vector3d* const ps, kIOS& bv); //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API void fit3(const Vector3d* const ps, kIOS& bv); //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API void fitn(const Vector3d* const ps, int n, kIOS& bv); //============================================================================== @@ -491,7 +482,6 @@ namespace OBBRSS_fit_functions { //============================================================================== template -FCL_EXPORT void fit1(const Vector3* const ps, OBBRSS& bv) { OBB_fit_functions::fit1(ps, bv.obb); @@ -500,7 +490,6 @@ void fit1(const Vector3* const ps, OBBRSS& bv) //============================================================================== template -FCL_EXPORT void fit2(const Vector3* const ps, OBBRSS& bv) { OBB_fit_functions::fit2(ps, bv.obb); @@ -509,7 +498,6 @@ void fit2(const Vector3* const ps, OBBRSS& bv) //============================================================================== template -FCL_EXPORT void fit3(const Vector3* const ps, OBBRSS& bv) { OBB_fit_functions::fit3(ps, bv.obb); @@ -518,7 +506,6 @@ void fit3(const Vector3* const ps, OBBRSS& bv) //============================================================================== template -FCL_EXPORT void fitn(const Vector3* const ps, int n, OBBRSS& bv) { OBB_fit_functions::fitn(ps, n, bv.obb); @@ -527,18 +514,22 @@ void fitn(const Vector3* const ps, int n, OBBRSS& bv) //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void fit1(const Vector3d* const ps, OBBRSS& bv); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void fit2(const Vector3d* const ps, OBBRSS& bv); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void fit3(const Vector3d* const ps, OBBRSS& bv); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void fitn(const Vector3d* const ps, int n, OBBRSS& bv); //============================================================================== @@ -547,7 +538,7 @@ void fitn(const Vector3d* const ps, int n, OBBRSS& bv); //============================================================================== template -struct FCL_EXPORT Fitter +struct Fitter { static void fit(const Vector3* const ps, int n, BV& bv) { @@ -558,7 +549,7 @@ struct FCL_EXPORT Fitter //============================================================================== template -struct FCL_EXPORT Fitter> +struct Fitter> { static void fit(const Vector3* const ps, int n, OBB& bv) { @@ -584,7 +575,7 @@ struct FCL_EXPORT Fitter> //============================================================================== template -struct FCL_EXPORT Fitter> +struct Fitter> { static void fit(const Vector3* const ps, int n, RSS& bv) { @@ -607,7 +598,7 @@ struct FCL_EXPORT Fitter> //============================================================================== template -struct FCL_EXPORT Fitter> +struct Fitter> { static void fit(const Vector3* const ps, int n, kIOS& bv) { @@ -630,7 +621,7 @@ struct FCL_EXPORT Fitter> //============================================================================== template -struct FCL_EXPORT Fitter> +struct Fitter> { static void fit(const Vector3* const ps, int n, OBBRSS& bv) { @@ -653,19 +644,19 @@ struct FCL_EXPORT Fitter> //============================================================================== extern template -struct Fitter>; +struct FCL_EXTERN_TEMPLATE_API Fitter>; //============================================================================== extern template -struct Fitter>; +struct FCL_EXTERN_TEMPLATE_API Fitter>; //============================================================================== extern template -struct Fitter>; +struct FCL_EXTERN_TEMPLATE_API Fitter>; //============================================================================== extern template -struct Fitter>; +struct FCL_EXTERN_TEMPLATE_API Fitter>; //============================================================================== } // namespace detail @@ -673,7 +664,6 @@ struct Fitter>; //============================================================================== template -FCL_EXPORT void fit(const Vector3* const ps, int n, BV& bv) { detail::Fitter::fit(ps, n, bv); @@ -686,7 +676,7 @@ namespace detail { /// @brief Convert a bounding volume of type BV1 in configuration tf1 to a /// bounding volume of type BV2 in I configuration. template -class FCL_EXPORT ConvertBVImpl +class ConvertBVImpl { private: static void run(const BV1& bv1, const Transform3& tf1, BV2& bv2) @@ -702,7 +692,7 @@ class FCL_EXPORT ConvertBVImpl //============================================================================== /// @brief Convert from AABB to AABB, not very tight but is fast. template -class FCL_EXPORT ConvertBVImpl, AABB> +class ConvertBVImpl, AABB> { public: static void run(const AABB& bv1, const Transform3& tf1, AABB& bv2) @@ -718,7 +708,7 @@ class FCL_EXPORT ConvertBVImpl, AABB> //============================================================================== template -class FCL_EXPORT ConvertBVImpl, OBB> +class ConvertBVImpl, OBB> { public: static void run(const AABB& bv1, const Transform3& tf1, OBB& bv2) @@ -767,7 +757,7 @@ class FCL_EXPORT ConvertBVImpl, OBB> //============================================================================== template -class FCL_EXPORT ConvertBVImpl, OBB> +class ConvertBVImpl, OBB> { public: static void run(const OBB& bv1, const Transform3& tf1, OBB& bv2) @@ -780,7 +770,7 @@ class FCL_EXPORT ConvertBVImpl, OBB> //============================================================================== template -class FCL_EXPORT ConvertBVImpl, OBB> +class ConvertBVImpl, OBB> { public: static void run(const OBBRSS& bv1, const Transform3& tf1, OBB& bv2) @@ -791,7 +781,7 @@ class FCL_EXPORT ConvertBVImpl, OBB> //============================================================================== template -class FCL_EXPORT ConvertBVImpl, OBB> +class ConvertBVImpl, OBB> { public: static void run(const RSS& bv1, const Transform3& tf1, OBB& bv2) @@ -804,7 +794,7 @@ class FCL_EXPORT ConvertBVImpl, OBB> //============================================================================== template -class FCL_EXPORT ConvertBVImpl> +class ConvertBVImpl> { public: static void run(const BV1& bv1, const Transform3& tf1, AABB& bv2) @@ -820,7 +810,7 @@ class FCL_EXPORT ConvertBVImpl> //============================================================================== template -class FCL_EXPORT ConvertBVImpl> +class ConvertBVImpl> { public: static void run(const BV1& bv1, const Transform3& tf1, OBB& bv2) @@ -833,7 +823,7 @@ class FCL_EXPORT ConvertBVImpl> //============================================================================== template -class FCL_EXPORT ConvertBVImpl, RSS> +class ConvertBVImpl, RSS> { public: static void run(const OBB& bv1, const Transform3& tf1, RSS& bv2) @@ -849,7 +839,7 @@ class FCL_EXPORT ConvertBVImpl, RSS> //============================================================================== template -class FCL_EXPORT ConvertBVImpl, RSS> +class ConvertBVImpl, RSS> { public: static void run(const RSS& bv1, const Transform3& tf1, RSS& bv2) @@ -865,7 +855,7 @@ class FCL_EXPORT ConvertBVImpl, RSS> //============================================================================== template -class FCL_EXPORT ConvertBVImpl, RSS> +class ConvertBVImpl, RSS> { public: static void run(const OBBRSS& bv1, const Transform3& tf1, RSS& bv2) @@ -876,7 +866,7 @@ class FCL_EXPORT ConvertBVImpl, RSS> //============================================================================== template -class FCL_EXPORT ConvertBVImpl, RSS> +class ConvertBVImpl, RSS> { public: static void run(const AABB& bv1, const Transform3& tf1, RSS& bv2) @@ -925,39 +915,39 @@ class FCL_EXPORT ConvertBVImpl, RSS> //============================================================================== extern template -class FCL_EXPORT ConvertBVImpl, AABB>; +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, AABB>; //============================================================================== extern template -class FCL_EXPORT ConvertBVImpl, OBB>; +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; //============================================================================== extern template -class FCL_EXPORT ConvertBVImpl, OBB>; +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; //============================================================================== extern template -class FCL_EXPORT ConvertBVImpl, OBB>; +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; //============================================================================== extern template -class FCL_EXPORT ConvertBVImpl, OBB>; +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; //============================================================================== extern template -class FCL_EXPORT ConvertBVImpl, RSS>; +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; //============================================================================== extern template -class FCL_EXPORT ConvertBVImpl, RSS>; +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; //============================================================================== extern template -class FCL_EXPORT ConvertBVImpl, RSS>; +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; //============================================================================== extern template -class FCL_EXPORT ConvertBVImpl, RSS>; +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; //============================================================================== } // namespace detail @@ -965,7 +955,6 @@ class FCL_EXPORT ConvertBVImpl, RSS>; //============================================================================== template -FCL_EXPORT void convertBV( const BV1& bv1, const Transform3& tf1, BV2& bv2) { diff --git a/include/fcl/math/bv/utility.h b/include/fcl/math/bv/utility.h index 2cea40cad..c10ffde76 100644 --- a/include/fcl/math/bv/utility.h +++ b/include/fcl/math/bv/utility.h @@ -46,13 +46,11 @@ namespace fcl /// @brief Compute a bounding volume that fits a set of n points. template -FCL_EXPORT void fit(const Vector3* const ps, int n, BV& bv); /// @brief Convert a bounding volume of type BV1 in configuration tf1 to /// bounding volume of type BV2 in identity configuration. template -FCL_EXPORT void convertBV( const BV1& bv1, const Transform3& tf1, BV2& bv2); diff --git a/include/fcl/math/constants.h b/include/fcl/math/constants.h index 27ce5e0c4..1f91be7d5 100644 --- a/include/fcl/math/constants.h +++ b/include/fcl/math/constants.h @@ -126,7 +126,7 @@ struct ScalarTrait { /// /// \tparam S The scalar type for which constant values will be retrieved. template -struct FCL_EXPORT constants +struct constants { typedef typename detail::ScalarTrait::type Real; diff --git a/include/fcl/math/detail/polysolver-inl.h b/include/fcl/math/detail/polysolver-inl.h index c2c538cce..24370c528 100644 --- a/include/fcl/math/detail/polysolver-inl.h +++ b/include/fcl/math/detail/polysolver-inl.h @@ -1,201 +1,201 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H -#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H - -#include "fcl/math/detail/polysolver.h" - -#include -#include "fcl/common/types.h" - -namespace fcl -{ - -namespace detail { - -//============================================================================== -extern template -class FCL_EXPORT PolySolver; - -//============================================================================== -template -int PolySolver::solveLinear(S c[2], S s[1]) -{ - if(isZero(c[1])) - return 0; - s[0] = - c[0] / c[1]; - return 1; -} - -//============================================================================== -template -int PolySolver::solveQuadric(S c[3], S s[2]) -{ - S p, q, D; - - // make sure we have a d2 equation - - if(isZero(c[2])) - return solveLinear(c, s); - - // normal for: x^2 + px + q - p = c[1] / (2.0 * c[2]); - q = c[0] / c[2]; - D = p * p - q; - - if(isZero(D)) - { - // one S root - s[0] = s[1] = -p; - return 1; - } - - if(D < 0.0) - // no real root - return 0; - else - { - // two real roots - S sqrt_D = sqrt(D); - s[0] = sqrt_D - p; - s[1] = -sqrt_D - p; - return 2; - } -} - -//============================================================================== -template -int PolySolver::solveCubic(S c[4], S s[3]) -{ - int i, num; - S sub, A, B, C, sq_A, p, q, cb_p, D; - const S ONE_OVER_THREE = 1 / 3.0; - const S PI = 3.14159265358979323846; - - // make sure we have a d2 equation - if(isZero(c[3])) - return solveQuadric(c, s); - - // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 - A = c[2] / c[3]; - B = c[1] / c[3]; - C = c[0] / c[3]; - - // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 - sq_A = A * A; - p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; - q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); - - // use Cardano's formula - cb_p = p * p * p; - D = q * q + cb_p; - - if(isZero(D)) - { - if(isZero(q)) - { - // one triple solution - s[0] = 0.0; - num = 1; - } - else - { - // one single and one S solution - S u = cbrt(-q); - s[0] = 2.0 * u; - s[1] = -u; - num = 2; - } - } - else - { - if(D < 0.0) - { - // three real solutions - S phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); - S t = 2.0 * sqrt(-p); - s[0] = t * cos(phi); - s[1] = -t * cos(phi + PI / 3.0); - s[2] = -t * cos(phi - PI / 3.0); - num = 3; - } - else - { - // one real solution - S sqrt_D = sqrt(D); - S u = cbrt(sqrt_D + fabs(q)); - if(q > 0.0) - s[0] = - u + p / u ; - else - s[0] = u - p / u; - num = 1; - } - } - - // re-substitute - sub = ONE_OVER_THREE * A; - for(i = 0; i < num; i++) - s[i] -= sub; - return num; -} - -//============================================================================== -template -bool PolySolver::isZero(S v) -{ - return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); -} - -//============================================================================== -template -bool PolySolver::cbrt(S v) -{ - return std::pow(v, 1.0 / 3.0); -} - -//============================================================================== -template -constexpr S PolySolver::getNearZeroThreshold() -{ - return 1e-9; -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H +#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H + +#include "fcl/math/detail/polysolver.h" + +#include +#include "fcl/common/types.h" + +namespace fcl +{ + +namespace detail { + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API PolySolver; + +//============================================================================== +template +int PolySolver::solveLinear(S c[2], S s[1]) +{ + if(isZero(c[1])) + return 0; + s[0] = - c[0] / c[1]; + return 1; +} + +//============================================================================== +template +int PolySolver::solveQuadric(S c[3], S s[2]) +{ + S p, q, D; + + // make sure we have a d2 equation + + if(isZero(c[2])) + return solveLinear(c, s); + + // normal for: x^2 + px + q + p = c[1] / (2.0 * c[2]); + q = c[0] / c[2]; + D = p * p - q; + + if(isZero(D)) + { + // one S root + s[0] = s[1] = -p; + return 1; + } + + if(D < 0.0) + // no real root + return 0; + else + { + // two real roots + S sqrt_D = sqrt(D); + s[0] = sqrt_D - p; + s[1] = -sqrt_D - p; + return 2; + } +} + +//============================================================================== +template +int PolySolver::solveCubic(S c[4], S s[3]) +{ + int i, num; + S sub, A, B, C, sq_A, p, q, cb_p, D; + const S ONE_OVER_THREE = 1 / 3.0; + const S PI = 3.14159265358979323846; + + // make sure we have a d2 equation + if(isZero(c[3])) + return solveQuadric(c, s); + + // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 + A = c[2] / c[3]; + B = c[1] / c[3]; + C = c[0] / c[3]; + + // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 + sq_A = A * A; + p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; + q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); + + // use Cardano's formula + cb_p = p * p * p; + D = q * q + cb_p; + + if(isZero(D)) + { + if(isZero(q)) + { + // one triple solution + s[0] = 0.0; + num = 1; + } + else + { + // one single and one S solution + S u = cbrt(-q); + s[0] = 2.0 * u; + s[1] = -u; + num = 2; + } + } + else + { + if(D < 0.0) + { + // three real solutions + S phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); + S t = 2.0 * sqrt(-p); + s[0] = t * cos(phi); + s[1] = -t * cos(phi + PI / 3.0); + s[2] = -t * cos(phi - PI / 3.0); + num = 3; + } + else + { + // one real solution + S sqrt_D = sqrt(D); + S u = cbrt(sqrt_D + fabs(q)); + if(q > 0.0) + s[0] = - u + p / u ; + else + s[0] = u - p / u; + num = 1; + } + } + + // re-substitute + sub = ONE_OVER_THREE * A; + for(i = 0; i < num; i++) + s[i] -= sub; + return num; +} + +//============================================================================== +template +bool PolySolver::isZero(S v) +{ + return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); +} + +//============================================================================== +template +bool PolySolver::cbrt(S v) +{ + return std::pow(v, 1.0 / 3.0); +} + +//============================================================================== +template +constexpr S PolySolver::getNearZeroThreshold() +{ + return 1e-9; +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/math/detail/polysolver.h b/include/fcl/math/detail/polysolver.h index 52808a9d7..d79217f2d 100644 --- a/include/fcl/math/detail/polysolver.h +++ b/include/fcl/math/detail/polysolver.h @@ -1,80 +1,80 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_H -#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_H - -#include "fcl/export.h" - -namespace fcl -{ - -namespace detail { - -/// @brief A class solves polynomial degree (1,2,3) equations -template -class FCL_EXPORT PolySolver -{ -public: - /// @brief Solve a linear equation with coefficients c, return roots s and number of roots - static int solveLinear(S c[2], S s[1]); - - /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots - static int solveQuadric(S c[3], S s[2]); - - /// @brief Solve a cubic function with coefficients c, return roots s and number of roots - static int solveCubic(S c[4], S s[3]); - -private: - /// @brief Check whether v is zero - static bool isZero(S v); - - /// @brief Compute v^{1/3} - static bool cbrt(S v); - - static constexpr S getNearZeroThreshold(); -}; - -using PolySolverf = PolySolver; -using PolySolverd = PolySolver; - -} // namespace detail -} // namespace fcl - -#include "fcl/math/detail/polysolver-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_H +#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_H + +#include "fcl/export.h" + +namespace fcl +{ + +namespace detail { + +/// @brief A class solves polynomial degree (1,2,3) equations +template +class PolySolver +{ +public: + /// @brief Solve a linear equation with coefficients c, return roots s and number of roots + static int solveLinear(S c[2], S s[1]); + + /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots + static int solveQuadric(S c[3], S s[2]); + + /// @brief Solve a cubic function with coefficients c, return roots s and number of roots + static int solveCubic(S c[4], S s[3]); + +private: + /// @brief Check whether v is zero + static bool isZero(S v); + + /// @brief Compute v^{1/3} + static bool cbrt(S v); + + static constexpr S getNearZeroThreshold(); +}; + +using PolySolverf = PolySolver; +using PolySolverd = PolySolver; + +} // namespace detail +} // namespace fcl + +#include "fcl/math/detail/polysolver-inl.h" + +#endif diff --git a/include/fcl/math/detail/project-inl.h b/include/fcl/math/detail/project-inl.h index abe4df6f7..9cccf5e51 100644 --- a/include/fcl/math/detail/project-inl.h +++ b/include/fcl/math/detail/project-inl.h @@ -1,320 +1,320 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_INL_H -#define FCL_NARROWPHASE_DETAIL_PROJECT_INL_H - -#include "fcl/math/detail/project.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -extern template -class FCL_EXPORT Project; - -//============================================================================== -template -typename Project::ProjectResult Project::projectLine(const Vector3& a, const Vector3& b, const Vector3& p) -{ - ProjectResult res; - - const Vector3 d = b - a; - const S l = d.squaredNorm(); - - if(l > 0) - { - const S t = (p - a).dot(d); - res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); - res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } - } - - return res; -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p) -{ - ProjectResult res; - - static const size_t nexti[3] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c}; - const Vector3 dl[] = {a - b, b - c, c - a}; - const Vector3& n = dl[0].cross(dl[1]); - const S l = n.squaredNorm(); - - if(l > 0) - { - S mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge - { - size_t j = nexti[i]; - ProjectResult res_line = projectLine(*vt[i], *vt[j], p); - - if(mindist < 0 || res_line.sqr_distance < mindist) - { - mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1< p_to_project = n * (d / l); - mindist = p_to_project.squaredNorm(); - res.encode = 7; // m = 0x111 - res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s; - res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s; - res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; - } - - res.sqr_distance = mindist; - } - - return res; - -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p) -{ - ProjectResult res; - - static const size_t nexti[] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c, &d}; - const Vector3 dl[3] = {a-d, b-d, c-d}; - S vl = triple(dl[0], dl[1], dl[2]); - bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0; - if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) - { - S mindist = -1; - - for(size_t i = 0; i < 3; ++i) - { - size_t j = nexti[i]; - S s = vl * (d-p).dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face - { - ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { - mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1< -typename Project::ProjectResult Project::projectLineOrigin(const Vector3& a, const Vector3& b) -{ - ProjectResult res; - - const Vector3 d = b - a; - const S l = d.squaredNorm(); - - if(l > 0) - { - const S t = - a.dot(d); - res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); - res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } - } - - return res; -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c) -{ - ProjectResult res; - - static const size_t nexti[3] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c}; - const Vector3 dl[] = {a - b, b - c, c - a}; - const Vector3& n = dl[0].cross(dl[1]); - const S l = n.squaredNorm(); - - if(l > 0) - { - S mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge - { - size_t j = nexti[i]; - ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); - - if(mindist < 0 || res_line.sqr_distance < mindist) - { - mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1< o_to_project = n * (d / l); - mindist = o_to_project.squaredNorm(); - res.encode = 7; // m = 0x111 - res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; - res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; - res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; - } - - res.sqr_distance = mindist; - } - - return res; - -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) -{ - ProjectResult res; - - static const size_t nexti[] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c, &d}; - const Vector3 dl[3] = {a-d, b-d, c-d}; - S vl = triple(dl[0], dl[1], dl[2]); - bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0; - if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) - { - S mindist = -1; - - for(size_t i = 0; i < 3; ++i) - { - size_t j = nexti[i]; - S s = vl * d.dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face - { - ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { - mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1< -Project::ProjectResult::ProjectResult() - : parameterization{0.0, 0.0, 0.0, 0.0}, sqr_distance(-1), encode(0) -{ - // Do nothing -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_INL_H +#define FCL_NARROWPHASE_DETAIL_PROJECT_INL_H + +#include "fcl/math/detail/project.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API Project; + +//============================================================================== +template +typename Project::ProjectResult Project::projectLine(const Vector3& a, const Vector3& b, const Vector3& p) +{ + ProjectResult res; + + const Vector3 d = b - a; + const S l = d.squaredNorm(); + + if(l > 0) + { + const S t = (p - a).dot(d); + res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); + res.parameterization[0] = 1 - res.parameterization[1]; + if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } + else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } + else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } + } + + return res; +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p) +{ + ProjectResult res; + + static const size_t nexti[3] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c}; + const Vector3 dl[] = {a - b, b - c, c - a}; + const Vector3& n = dl[0].cross(dl[1]); + const S l = n.squaredNorm(); + + if(l > 0) + { + S mindist = -1; + for(size_t i = 0; i < 3; ++i) + { + if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + { + size_t j = nexti[i]; + ProjectResult res_line = projectLine(*vt[i], *vt[j], p); + + if(mindist < 0 || res_line.sqr_distance < mindist) + { + mindist = res_line.sqr_distance; + res.encode = static_cast(((res_line.encode&1)?1< p_to_project = n * (d / l); + mindist = p_to_project.squaredNorm(); + res.encode = 7; // m = 0x111 + res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s; + res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s; + res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + } + + res.sqr_distance = mindist; + } + + return res; + +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p) +{ + ProjectResult res; + + static const size_t nexti[] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c, &d}; + const Vector3 dl[3] = {a-d, b-d, c-d}; + S vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0; + if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + { + S mindist = -1; + + for(size_t i = 0; i < 3; ++i) + { + size_t j = nexti[i]; + S s = vl * (d-p).dot(dl[i].cross(dl[j])); + if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + { + ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); + if(mindist < 0 || res_triangle.sqr_distance < mindist) + { + mindist = res_triangle.sqr_distance; + res.encode = static_cast( (res_triangle.encode&1?1< +typename Project::ProjectResult Project::projectLineOrigin(const Vector3& a, const Vector3& b) +{ + ProjectResult res; + + const Vector3 d = b - a; + const S l = d.squaredNorm(); + + if(l > 0) + { + const S t = - a.dot(d); + res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); + res.parameterization[0] = 1 - res.parameterization[1]; + if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } + else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } + else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } + } + + return res; +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c) +{ + ProjectResult res; + + static const size_t nexti[3] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c}; + const Vector3 dl[] = {a - b, b - c, c - a}; + const Vector3& n = dl[0].cross(dl[1]); + const S l = n.squaredNorm(); + + if(l > 0) + { + S mindist = -1; + for(size_t i = 0; i < 3; ++i) + { + if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + { + size_t j = nexti[i]; + ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); + + if(mindist < 0 || res_line.sqr_distance < mindist) + { + mindist = res_line.sqr_distance; + res.encode = static_cast(((res_line.encode&1)?1< o_to_project = n * (d / l); + mindist = o_to_project.squaredNorm(); + res.encode = 7; // m = 0x111 + res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; + res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; + res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + } + + res.sqr_distance = mindist; + } + + return res; + +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) +{ + ProjectResult res; + + static const size_t nexti[] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c, &d}; + const Vector3 dl[3] = {a-d, b-d, c-d}; + S vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0; + if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + { + S mindist = -1; + + for(size_t i = 0; i < 3; ++i) + { + size_t j = nexti[i]; + S s = vl * d.dot(dl[i].cross(dl[j])); + if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + { + ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); + if(mindist < 0 || res_triangle.sqr_distance < mindist) + { + mindist = res_triangle.sqr_distance; + res.encode = static_cast( (res_triangle.encode&1?1< +Project::ProjectResult::ProjectResult() + : parameterization{0.0, 0.0, 0.0, 0.0}, sqr_distance(-1), encode(0) +{ + // Do nothing +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/math/detail/project.h b/include/fcl/math/detail/project.h index 39311f2b9..c08f065da 100644 --- a/include/fcl/math/detail/project.h +++ b/include/fcl/math/detail/project.h @@ -1,96 +1,96 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_H -#define FCL_NARROWPHASE_DETAIL_PROJECT_H - -#include "fcl/common/types.h" -#include "fcl/math/geometry.h" - -namespace fcl -{ - -namespace detail -{ - -/// @brief Project functions -template -class FCL_EXPORT Project -{ -public: - struct ProjectResult - { - /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array) - S parameterization[4]; - - /// @brief square distance from the query point to the projected simplex - S sqr_distance; - - /// @brief the code of the projection type - unsigned int encode; - - ProjectResult(); - }; - - /// @brief Project point p onto line a-b - static ProjectResult projectLine(const Vector3& a, const Vector3& b, const Vector3& p); - - /// @brief Project point p onto triangle a-b-c - static ProjectResult projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p); - - /// @brief Project point p onto tetrahedra a-b-c-d - static ProjectResult projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p); - - /// @brief Project origin (0) onto line a-b - static ProjectResult projectLineOrigin(const Vector3& a, const Vector3& b); - - /// @brief Project origin (0) onto triangle a-b-c - static ProjectResult projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c); - - /// @brief Project origin (0) onto tetrahedran a-b-c-d - static ProjectResult projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d); -}; - -using Projectf = Project; -using Projectd = Project; - -} // namespace detail -} // namespace fcl - -#include "fcl/math/detail/project-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_H +#define FCL_NARROWPHASE_DETAIL_PROJECT_H + +#include "fcl/common/types.h" +#include "fcl/math/geometry.h" + +namespace fcl +{ + +namespace detail +{ + +/// @brief Project functions +template +class Project +{ +public: + struct ProjectResult + { + /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array) + S parameterization[4]; + + /// @brief square distance from the query point to the projected simplex + S sqr_distance; + + /// @brief the code of the projection type + unsigned int encode; + + ProjectResult(); + }; + + /// @brief Project point p onto line a-b + static ProjectResult projectLine(const Vector3& a, const Vector3& b, const Vector3& p); + + /// @brief Project point p onto triangle a-b-c + static ProjectResult projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p); + + /// @brief Project point p onto tetrahedra a-b-c-d + static ProjectResult projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p); + + /// @brief Project origin (0) onto line a-b + static ProjectResult projectLineOrigin(const Vector3& a, const Vector3& b); + + /// @brief Project origin (0) onto triangle a-b-c + static ProjectResult projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c); + + /// @brief Project origin (0) onto tetrahedran a-b-c-d + static ProjectResult projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d); +}; + +using Projectf = Project; +using Projectd = Project; + +} // namespace detail +} // namespace fcl + +#include "fcl/math/detail/project-inl.h" + +#endif diff --git a/include/fcl/math/detail/seed.h b/include/fcl/math/detail/seed.h index 3ec4eeb68..df76dd88e 100644 --- a/include/fcl/math/detail/seed.h +++ b/include/fcl/math/detail/seed.h @@ -46,7 +46,7 @@ namespace fcl namespace detail { -class FCL_EXPORT Seed +class FCL_API Seed { public: diff --git a/include/fcl/math/geometry-inl.h b/include/fcl/math/geometry-inl.h index 2aa5ffa74..d0ab73d12 100644 --- a/include/fcl/math/geometry-inl.h +++ b/include/fcl/math/geometry-inl.h @@ -44,41 +44,50 @@ namespace fcl { //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void normalize(Vector3d& v, bool* signal); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void hat(Matrix3d& mat, const Vector3d& vec); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void eigen(const Matrix3d& m, Vector3d& dout, Matrix3d& vout); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void eigen_old(const Matrix3d& m, Vector3d& dout, Matrix3d& vout); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void axisFromEigen( const Matrix3d& eigenV, const Vector3d& eigenS, Matrix3d& axis); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void axisFromEigen(const Matrix3d& eigenV, const Vector3d& eigenS, Transform3d& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void generateCoordinateSystem(Matrix3d& axis); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void generateCoordinateSystem(Transform3d& tf); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void getRadiusAndOriginAndRectangleSize( const Vector3d* const ps, const Vector3d* const ps2, @@ -92,6 +101,7 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void getRadiusAndOriginAndRectangleSize( const Vector3d* const ps, const Vector3d* const ps2, @@ -104,6 +114,7 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void circumCircleComputation( const Vector3d& a, const Vector3d& b, @@ -113,6 +124,7 @@ void circumCircleComputation( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double maximumDistance( const Vector3d* const ps, const Vector3d* const ps2, @@ -123,6 +135,7 @@ double maximumDistance( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void getExtentAndCenter( const Vector3d* const ps, const Vector3d* const ps2, @@ -135,6 +148,7 @@ void getExtentAndCenter( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void getCovariance( const Vector3d* const ps, const Vector3d* const ps2, @@ -148,7 +162,6 @@ namespace detail { //============================================================================== template -FCL_EXPORT S maximumDistance_mesh( const Vector3* const ps, const Vector3* const ps2, @@ -193,7 +206,6 @@ S maximumDistance_mesh( //============================================================================== template -FCL_EXPORT S maximumDistance_pointcloud( const Vector3* const ps, const Vector3* const ps2, @@ -228,7 +240,6 @@ S maximumDistance_pointcloud( /// @brief Compute the bounding volume extent and center for a set or subset of /// points. The bounding volume axes are known. template -FCL_EXPORT void getExtentAndCenter_pointcloud( const Vector3* const ps, const Vector3* const ps2, @@ -293,7 +304,6 @@ void getExtentAndCenter_pointcloud( /// @brief Compute the bounding volume extent and center for a set or subset of /// points. The bounding volume axes are known. template -FCL_EXPORT void getExtentAndCenter_mesh( const Vector3* const ps, const Vector3* const ps2, @@ -366,6 +376,7 @@ void getExtentAndCenter_mesh( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double maximumDistance_mesh( const Vector3d* const ps, const Vector3d* const ps2, @@ -376,6 +387,7 @@ double maximumDistance_mesh( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double maximumDistance_pointcloud( const Vector3d* const ps, const Vector3d* const ps2, @@ -385,6 +397,7 @@ double maximumDistance_pointcloud( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void getExtentAndCenter_pointcloud( const Vector3d* const ps, const Vector3d* const ps2, @@ -396,6 +409,7 @@ void getExtentAndCenter_pointcloud( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void getExtentAndCenter_mesh( const Vector3d* const ps, const Vector3d* const ps2, @@ -412,7 +426,6 @@ void getExtentAndCenter_mesh( //============================================================================== template -FCL_EXPORT void normalize(Vector3& v, bool* signal) { S sqr_length = v.squaredNorm(); @@ -430,7 +443,6 @@ void normalize(Vector3& v, bool* signal) //============================================================================== template -FCL_EXPORT typename Derived::RealScalar triple(const Eigen::MatrixBase& x, const Eigen::MatrixBase& y, const Eigen::MatrixBase& z) @@ -440,7 +452,6 @@ typename Derived::RealScalar triple(const Eigen::MatrixBase& x, //============================================================================== template -FCL_EXPORT void generateCoordinateSystem( const Eigen::MatrixBase& w, Eigen::MatrixBase& u, @@ -472,7 +483,6 @@ void generateCoordinateSystem( //============================================================================== template -FCL_EXPORT VectorN combine( const VectorN& v1, const VectorN& v2) { @@ -484,7 +494,6 @@ VectorN combine( //============================================================================== template -FCL_EXPORT void hat(Matrix3& mat, const Vector3& vec) { mat << 0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0; @@ -492,7 +501,6 @@ void hat(Matrix3& mat, const Vector3& vec) //============================================================================== template -FCL_EXPORT void eigen(const Matrix3& m, Vector3& dout, Matrix3& vout) { // We assume that m is symmetric matrix. @@ -508,7 +516,6 @@ void eigen(const Matrix3& m, Vector3& dout, Matrix3& vout) //============================================================================== template -FCL_EXPORT void eigen_old(const Matrix3& m, Vector3& dout, Matrix3& vout) { Matrix3 R(m); @@ -598,7 +605,6 @@ void eigen_old(const Matrix3& m, Vector3& dout, Matrix3& vout) //============================================================================== template -FCL_EXPORT void axisFromEigen(const Matrix3& eigenV, const Vector3& eigenS, Matrix3& axis) @@ -638,7 +644,6 @@ void axisFromEigen(const Matrix3& eigenV, //============================================================================== template -FCL_EXPORT void axisFromEigen(const Matrix3& eigenV, const Vector3& eigenS, Transform3& tf) @@ -678,7 +683,6 @@ void axisFromEigen(const Matrix3& eigenV, //============================================================================== template -FCL_EXPORT void generateCoordinateSystem(Matrix3& axis) { // Assum axis.col(0) is closest to z-axis @@ -724,7 +728,6 @@ void generateCoordinateSystem(Matrix3& axis) //============================================================================== template -FCL_EXPORT void generateCoordinateSystem(Transform3& tf) { // Assum axis.col(0) is closest to z-axis @@ -770,7 +773,6 @@ void generateCoordinateSystem(Transform3& tf) //============================================================================== template -FCL_EXPORT void relativeTransform( const Eigen::MatrixBase& R1, const Eigen::MatrixBase& t1, const Eigen::MatrixBase& R2, const Eigen::MatrixBase& t2, @@ -802,7 +804,6 @@ void relativeTransform( //============================================================================== template -FCL_EXPORT void relativeTransform( const Transform3& T1, const Transform3& T2, @@ -824,7 +825,6 @@ void relativeTransform( //============================================================================== template -FCL_EXPORT void getRadiusAndOriginAndRectangleSize( const Vector3* const ps, const Vector3* const ps2, @@ -1107,7 +1107,6 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== template -FCL_EXPORT void getRadiusAndOriginAndRectangleSize( const Vector3* const ps, const Vector3* const ps2, @@ -1389,7 +1388,6 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== template -FCL_EXPORT void circumCircleComputation( const Vector3& a, const Vector3& b, @@ -1414,7 +1412,6 @@ void circumCircleComputation( //============================================================================== template -FCL_EXPORT S maximumDistance( const Vector3* const ps, const Vector3* const ps2, @@ -1431,7 +1428,6 @@ S maximumDistance( //============================================================================== template -FCL_EXPORT void getExtentAndCenter( const Vector3* const ps, const Vector3* const ps2, @@ -1450,7 +1446,6 @@ void getExtentAndCenter( //============================================================================== template -FCL_EXPORT void getCovariance( const Vector3* const ps, const Vector3* const ps2, diff --git a/include/fcl/math/geometry.h b/include/fcl/math/geometry.h index c119c749f..4a1162b35 100644 --- a/include/fcl/math/geometry.h +++ b/include/fcl/math/geometry.h @@ -49,72 +49,59 @@ namespace fcl { template -FCL_EXPORT void normalize(Vector3& v, bool* signal); template -FCL_EXPORT typename Derived::RealScalar triple(const Eigen::MatrixBase& x, const Eigen::MatrixBase& y, const Eigen::MatrixBase& z); template -FCL_EXPORT void generateCoordinateSystem( const Eigen::MatrixBase& w, Eigen::MatrixBase& u, Eigen::MatrixBase& v); template -FCL_EXPORT VectorN combine( const VectorN& v1, const VectorN& v2); template -FCL_EXPORT void hat(Matrix3& mat, const Vector3& vec); /// @brief compute the eigen vector and eigen vector of a matrix. dout is the /// eigen values, vout is the eigen vectors template -FCL_EXPORT void eigen(const Matrix3& m, Vector3& dout, Matrix3& vout); /// @brief compute the eigen vector and eigen vector of a matrix. dout is the /// eigen values, vout is the eigen vectors template -FCL_EXPORT void eigen_old(const Matrix3& m, Vector3& dout, Matrix3& vout); template -FCL_EXPORT void axisFromEigen(const Matrix3& eigenV, const Vector3& eigenS, Matrix3& axis); template -FCL_EXPORT void axisFromEigen(const Matrix3& eigenV, const Vector3& eigenS, Transform3& tf); template -FCL_EXPORT void generateCoordinateSystem(Matrix3& axis); template -FCL_EXPORT void generateCoordinateSystem(Transform3& tf); template -FCL_EXPORT void relativeTransform( const Eigen::MatrixBase& R1, const Eigen::MatrixBase& t1, const Eigen::MatrixBase& R2, const Eigen::MatrixBase& t2, Eigen::MatrixBase& R, Eigen::MatrixBase& t); template -FCL_EXPORT void relativeTransform( const Eigen::Transform& T1, const Eigen::Transform& T2, @@ -123,7 +110,6 @@ void relativeTransform( /// @brief Compute the RSS bounding volume parameters: radius, rectangle size /// and the origin, given the BV axises. template -FCL_EXPORT void getRadiusAndOriginAndRectangleSize( const Vector3* const ps, const Vector3* const ps2, @@ -138,7 +124,6 @@ void getRadiusAndOriginAndRectangleSize( /// @brief Compute the RSS bounding volume parameters: radius, rectangle size /// and the origin, given the BV axises. template -FCL_EXPORT void getRadiusAndOriginAndRectangleSize( const Vector3* const ps, const Vector3* const ps2, @@ -151,7 +136,6 @@ void getRadiusAndOriginAndRectangleSize( /// @brief Compute the center and radius for a triangle's circumcircle template -FCL_EXPORT void circumCircleComputation( const Vector3& a, const Vector3& b, @@ -161,7 +145,6 @@ void circumCircleComputation( /// @brief Compute the maximum distance from a given center point to a point cloud template -FCL_EXPORT S maximumDistance( const Vector3* const ps, const Vector3* const ps2, @@ -173,7 +156,6 @@ S maximumDistance( /// @brief Compute the bounding volume extent and center for a set or subset of /// points, given the BV axises. template -FCL_EXPORT void getExtentAndCenter( const Vector3* const ps, const Vector3* const ps2, @@ -187,7 +169,6 @@ void getExtentAndCenter( /// @brief Compute the bounding volume extent and center for a set or subset of /// points, given the BV axises. template -FCL_EXPORT void getExtentAndCenter( const Vector3* const ps, const Vector3* const ps2, @@ -201,7 +182,6 @@ void getExtentAndCenter( /// ts = null, then indices refer to points directly; otherwise refer to /// triangles template -FCL_EXPORT void getCovariance( const Vector3* const ps, const Vector3* const ps2, diff --git a/include/fcl/math/motion/bv_motion_bound_visitor.h b/include/fcl/math/motion/bv_motion_bound_visitor.h index cecfe06e7..e60d3c218 100644 --- a/include/fcl/math/motion/bv_motion_bound_visitor.h +++ b/include/fcl/math/motion/bv_motion_bound_visitor.h @@ -59,7 +59,7 @@ class TranslationMotion; /// @brief Compute the motion bound for a bounding volume, given the closest /// direction n between two query objects template -class FCL_EXPORT BVMotionBoundVisitor +class BVMotionBoundVisitor { public: virtual S visit(const MotionBase& motion) const = 0; diff --git a/include/fcl/math/motion/interp_motion-inl.h b/include/fcl/math/motion/interp_motion-inl.h index 384367a80..e25b99fe6 100644 --- a/include/fcl/math/motion/interp_motion-inl.h +++ b/include/fcl/math/motion/interp_motion-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT InterpMotion; +class FCL_EXTERN_TEMPLATE_API InterpMotion; //============================================================================== template diff --git a/include/fcl/math/motion/interp_motion.h b/include/fcl/math/motion/interp_motion.h index a7f58eb74..55ec717c9 100644 --- a/include/fcl/math/motion/interp_motion.h +++ b/include/fcl/math/motion/interp_motion.h @@ -55,7 +55,7 @@ namespace fcl /// T(0) = T0 + R0 p_ref - p_ref /// T(1) = T1 + R1 p_ref - p_ref template -class FCL_EXPORT InterpMotion : public MotionBase +class InterpMotion : public MotionBase { public: /// @brief Default transformations are all identities @@ -81,7 +81,7 @@ class FCL_EXPORT InterpMotion : public MotionBase /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const; - /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor + /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor S computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const; /// @brief Get the rotation and translation in current step @@ -94,9 +94,9 @@ class FCL_EXPORT InterpMotion : public MotionBase void computeVelocity(); Quaternion deltaRotation(S dt) const; - + Quaternion absoluteRotation(S dt) const; - + /// @brief The transformation at time 0 Transform3 tf1; diff --git a/include/fcl/math/motion/motion_base-inl.h b/include/fcl/math/motion/motion_base-inl.h index 280058f73..88bde2bd9 100644 --- a/include/fcl/math/motion/motion_base-inl.h +++ b/include/fcl/math/motion/motion_base-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT MotionBase; +class FCL_EXTERN_TEMPLATE_API MotionBase; //============================================================================== template diff --git a/include/fcl/math/motion/screw_motion-inl.h b/include/fcl/math/motion/screw_motion-inl.h index 53c5c5bfd..13650400c 100644 --- a/include/fcl/math/motion/screw_motion-inl.h +++ b/include/fcl/math/motion/screw_motion-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT ScrewMotion; +class FCL_EXTERN_TEMPLATE_API ScrewMotion; //============================================================================== template diff --git a/include/fcl/math/motion/screw_motion.h b/include/fcl/math/motion/screw_motion.h index fce33874e..0adb7fa95 100644 --- a/include/fcl/math/motion/screw_motion.h +++ b/include/fcl/math/motion/screw_motion.h @@ -49,7 +49,7 @@ namespace fcl { template -class FCL_EXPORT ScrewMotion : public MotionBase +class ScrewMotion : public MotionBase { public: /// @brief Default transformations are all identities diff --git a/include/fcl/math/motion/spline_motion-inl.h b/include/fcl/math/motion/spline_motion-inl.h index 9986aecb0..cecb3bd6d 100644 --- a/include/fcl/math/motion/spline_motion-inl.h +++ b/include/fcl/math/motion/spline_motion-inl.h @@ -47,7 +47,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SplineMotion; +class FCL_EXTERN_TEMPLATE_API SplineMotion; //============================================================================== template diff --git a/include/fcl/math/motion/spline_motion.h b/include/fcl/math/motion/spline_motion.h index 96f2eba24..7267f4d5f 100644 --- a/include/fcl/math/motion/spline_motion.h +++ b/include/fcl/math/motion/spline_motion.h @@ -50,7 +50,7 @@ namespace fcl { template -class FCL_EXPORT SplineMotion : public MotionBase +class SplineMotion : public MotionBase { public: /// @brief Construct motion from 4 deBoor points @@ -63,7 +63,7 @@ class FCL_EXPORT SplineMotion : public MotionBase SplineMotion(const Transform3& tf1, const Transform3& tf2); - + /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. bool integrate(S dt) const override; @@ -86,7 +86,7 @@ class FCL_EXPORT SplineMotion : public MotionBase S getWeight1(S t) const; S getWeight2(S t) const; S getWeight3(S t) const; - + Vector3 Td[4]; Vector3 Rd[4]; @@ -102,7 +102,7 @@ class FCL_EXPORT SplineMotion : public MotionBase public: S computeTBound(const Vector3& n) const; - + S computeDWMax() const; S getCurrentTime() const; diff --git a/include/fcl/math/motion/taylor_model/interval-inl.h b/include/fcl/math/motion/taylor_model/interval-inl.h index 2e2dbb105..d1350cf0e 100644 --- a/include/fcl/math/motion/taylor_model/interval-inl.h +++ b/include/fcl/math/motion/taylor_model/interval-inl.h @@ -46,14 +46,16 @@ namespace fcl //============================================================================== extern template -struct Interval; +struct FCL_EXTERN_TEMPLATE_API Interval; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API Interval bound(const Interval& i, double v); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API Interval bound(const Interval& i, const Interval& other); //============================================================================== diff --git a/include/fcl/math/motion/taylor_model/interval.h b/include/fcl/math/motion/taylor_model/interval.h index cda1aa732..5aa2dc52f 100644 --- a/include/fcl/math/motion/taylor_model/interval.h +++ b/include/fcl/math/motion/taylor_model/interval.h @@ -47,7 +47,7 @@ namespace fcl /// @brief Interval class for [a, b] template -struct FCL_EXPORT Interval +struct Interval { S i_[2]; @@ -125,11 +125,9 @@ struct FCL_EXPORT Interval }; template -FCL_EXPORT Interval bound(const Interval& i, S v); template -FCL_EXPORT Interval bound(const Interval& i, const Interval& other); } // namespace fcl diff --git a/include/fcl/math/motion/taylor_model/interval_matrix-inl.h b/include/fcl/math/motion/taylor_model/interval_matrix-inl.h index bda8fc03e..0ce69295c 100644 --- a/include/fcl/math/motion/taylor_model/interval_matrix-inl.h +++ b/include/fcl/math/motion/taylor_model/interval_matrix-inl.h @@ -45,10 +45,11 @@ namespace fcl //============================================================================== extern template -struct IMatrix3; +struct FCL_EXTERN_TEMPLATE_API IMatrix3; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API IMatrix3 rotationConstrain(const IMatrix3& m); //============================================================================== diff --git a/include/fcl/math/motion/taylor_model/interval_matrix.h b/include/fcl/math/motion/taylor_model/interval_matrix.h index eab03c2d4..3f16c3eca 100644 --- a/include/fcl/math/motion/taylor_model/interval_matrix.h +++ b/include/fcl/math/motion/taylor_model/interval_matrix.h @@ -45,7 +45,7 @@ namespace fcl { template -struct FCL_EXPORT IMatrix3 +struct IMatrix3 { IVector3 v_[3]; @@ -95,7 +95,6 @@ struct FCL_EXPORT IMatrix3 }; template -FCL_EXPORT IMatrix3 rotationConstrain(const IMatrix3& m); } // namespace fcl diff --git a/include/fcl/math/motion/taylor_model/interval_vector-inl.h b/include/fcl/math/motion/taylor_model/interval_vector-inl.h index 5755f763a..ffcc77e15 100644 --- a/include/fcl/math/motion/taylor_model/interval_vector-inl.h +++ b/include/fcl/math/motion/taylor_model/interval_vector-inl.h @@ -45,14 +45,16 @@ namespace fcl //============================================================================== extern template -struct IVector3; +struct FCL_EXTERN_TEMPLATE_API IVector3; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API IVector3 bound(const IVector3& i, const Vector3& v); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API IVector3 bound(const IVector3& i, const IVector3& v); //============================================================================== diff --git a/include/fcl/math/motion/taylor_model/interval_vector.h b/include/fcl/math/motion/taylor_model/interval_vector.h index d5e5a9415..d4d3473f9 100644 --- a/include/fcl/math/motion/taylor_model/interval_vector.h +++ b/include/fcl/math/motion/taylor_model/interval_vector.h @@ -44,7 +44,7 @@ namespace fcl { template -struct FCL_EXPORT IVector3 +struct IVector3 { Interval i_[3]; @@ -72,7 +72,7 @@ struct FCL_EXPORT IVector3 void setValue(const Vector3& v); void setValue(S v[3]); - + IVector3 operator + (const IVector3& other) const; IVector3& operator += (const IVector3& other); @@ -90,7 +90,7 @@ struct FCL_EXPORT IVector3 Interval& operator [] (size_t i); Vector3 getLow() const; - + Vector3 getHigh() const; void print() const; @@ -106,11 +106,9 @@ struct FCL_EXPORT IVector3 }; template -FCL_EXPORT IVector3 bound(const IVector3& i, const Vector3& v); template -FCL_EXPORT IVector3 bound(const IVector3& i, const IVector3& v); } // namespace fcl diff --git a/include/fcl/math/motion/taylor_model/taylor_matrix-inl.h b/include/fcl/math/motion/taylor_model/taylor_matrix-inl.h index f5b184375..592b00c4d 100644 --- a/include/fcl/math/motion/taylor_model/taylor_matrix-inl.h +++ b/include/fcl/math/motion/taylor_model/taylor_matrix-inl.h @@ -45,34 +45,41 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT TMatrix3; +class FCL_EXTERN_TEMPLATE_API TMatrix3; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TMatrix3 rotationConstrain(const TMatrix3& m); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TMatrix3 operator * (const Matrix3& m, const TaylorModel& a); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TMatrix3 operator * (const TaylorModel& a, const Matrix3& m); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TMatrix3 operator * (double d, const TMatrix3& m); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TMatrix3 operator + (const Matrix3& m1, const TMatrix3& m2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TMatrix3 operator - (const Matrix3& m1, const TMatrix3& m2); //============================================================================== diff --git a/include/fcl/math/motion/taylor_model/taylor_matrix.h b/include/fcl/math/motion/taylor_model/taylor_matrix.h index c26f53309..eb1180582 100644 --- a/include/fcl/math/motion/taylor_model/taylor_matrix.h +++ b/include/fcl/math/motion/taylor_model/taylor_matrix.h @@ -45,10 +45,10 @@ namespace fcl { template -class FCL_EXPORT TMatrix3 +class TMatrix3 { TVector3 v_[3]; - + public: TMatrix3(); TMatrix3(const std::shared_ptr>& time_interval); @@ -106,31 +106,24 @@ class FCL_EXPORT TMatrix3 }; template -FCL_EXPORT TMatrix3 rotationConstrain(const TMatrix3& m); template -FCL_EXPORT TMatrix3 operator * (const Matrix3& m, const TaylorModel& a); template -FCL_EXPORT TMatrix3 operator * (const TaylorModel& a, const Matrix3& m); template -FCL_EXPORT TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m); template -FCL_EXPORT TMatrix3 operator * (S d, const TMatrix3& m); template -FCL_EXPORT TMatrix3 operator + (const Matrix3& m1, const TMatrix3& m2); template -FCL_EXPORT TMatrix3 operator - (const Matrix3& m1, const TMatrix3& m2); } // namespace fcl diff --git a/include/fcl/math/motion/taylor_model/taylor_model-inl.h b/include/fcl/math/motion/taylor_model/taylor_model-inl.h index 861f72d0f..8d37c4d52 100644 --- a/include/fcl/math/motion/taylor_model/taylor_model-inl.h +++ b/include/fcl/math/motion/taylor_model/taylor_model-inl.h @@ -48,30 +48,36 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT TaylorModel; +class FCL_EXTERN_TEMPLATE_API TaylorModel; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TaylorModel operator * (double d, const TaylorModel& a); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TaylorModel operator + (double d, const TaylorModel& a); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TaylorModel operator - (double d, const TaylorModel& a); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void generateTaylorModelForCosFunc(TaylorModel& tm, double w, double q0); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void generateTaylorModelForSinFunc(TaylorModel& tm, double w, double q0); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void generateTaylorModelForLinearFunc(TaylorModel& tm, double p, double v); //============================================================================== diff --git a/include/fcl/math/motion/taylor_model/taylor_model.h b/include/fcl/math/motion/taylor_model/taylor_model.h index b8c199949..8b5120318 100644 --- a/include/fcl/math/motion/taylor_model/taylor_model.h +++ b/include/fcl/math/motion/taylor_model/taylor_model.h @@ -55,7 +55,7 @@ namespace fcl /// remainder. All the operations on two Taylor models assume their time /// intervals are the same. template -class FCL_EXPORT TaylorModel +class TaylorModel { /// @brief time interval std::shared_ptr> time_interval_; @@ -69,7 +69,7 @@ class FCL_EXPORT TaylorModel public: void setTimeInterval(S l, S r); - + void setTimeInterval(const std::shared_ptr>& time_interval); const std::shared_ptr>& getTimeInterval() const; @@ -78,7 +78,7 @@ class FCL_EXPORT TaylorModel S& coeff(std::size_t i); const Interval& remainder() const; Interval& remainder(); - + TaylorModel(); TaylorModel(const std::shared_ptr>& time_interval); TaylorModel(S coeff, const std::shared_ptr>& time_interval); @@ -118,30 +118,24 @@ class FCL_EXPORT TaylorModel }; template -FCL_EXPORT TaylorModel operator * (S d, const TaylorModel& a); template -FCL_EXPORT TaylorModel operator + (S d, const TaylorModel& a); template -FCL_EXPORT TaylorModel operator - (S d, const TaylorModel& a); /// @brief Generate Taylor model for cos(w t + q0) template -FCL_EXPORT void generateTaylorModelForCosFunc(TaylorModel& tm, S w, S q0); /// @brief Generate Taylor model for sin(w t + q0) template -FCL_EXPORT void generateTaylorModelForSinFunc(TaylorModel& tm, S w, S q0); /// @brief Generate Taylor model for p + v t template -FCL_EXPORT void generateTaylorModelForLinearFunc(TaylorModel& tm, S p, S v); } // namespace fcl diff --git a/include/fcl/math/motion/taylor_model/taylor_vector-inl.h b/include/fcl/math/motion/taylor_model/taylor_vector-inl.h index 1ce2b3613..1bcda4e6b 100644 --- a/include/fcl/math/motion/taylor_model/taylor_vector-inl.h +++ b/include/fcl/math/motion/taylor_model/taylor_vector-inl.h @@ -45,22 +45,26 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT TVector3; +class FCL_EXTERN_TEMPLATE_API TVector3; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void generateTVector3ForLinearFunc(TVector3& v, const Vector3& position, const Vector3& velocity); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TVector3 operator * (const Vector3& v, const TaylorModel& a); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TVector3 operator + (const Vector3& v1, const TVector3& v2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API TVector3 operator - (const Vector3& v1, const TVector3& v2); //============================================================================== diff --git a/include/fcl/math/motion/taylor_model/taylor_vector.h b/include/fcl/math/motion/taylor_model/taylor_vector.h index 7df662f70..72a18172b 100644 --- a/include/fcl/math/motion/taylor_model/taylor_vector.h +++ b/include/fcl/math/motion/taylor_model/taylor_vector.h @@ -45,18 +45,18 @@ namespace fcl { template -class FCL_EXPORT TVector3 +class TVector3 { TaylorModel i_[3]; public: - + TVector3(); TVector3(const std::shared_ptr>& time_interval); TVector3(TaylorModel v[3]); TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2); TVector3(const Vector3& v, const std::shared_ptr>& time_interval); - + TVector3 operator + (const TVector3& other) const; TVector3& operator += (const TVector3& other); @@ -104,19 +104,15 @@ class FCL_EXPORT TVector3 }; template -FCL_EXPORT void generateTVector3ForLinearFunc(TVector3& v, const Vector3& position, const Vector3& velocity); template -FCL_EXPORT TVector3 operator * (const Vector3& v, const TaylorModel& a); template -FCL_EXPORT TVector3 operator + (const Vector3& v1, const TVector3& v2); template -FCL_EXPORT TVector3 operator - (const Vector3& v1, const TVector3& v2); } // namespace fcl diff --git a/include/fcl/math/motion/taylor_model/time_interval-inl.h b/include/fcl/math/motion/taylor_model/time_interval-inl.h index 62d83d034..2aead4d33 100644 --- a/include/fcl/math/motion/taylor_model/time_interval-inl.h +++ b/include/fcl/math/motion/taylor_model/time_interval-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -struct TimeInterval; +struct FCL_EXTERN_TEMPLATE_API TimeInterval; //============================================================================== template diff --git a/include/fcl/math/motion/taylor_model/time_interval.h b/include/fcl/math/motion/taylor_model/time_interval.h index 1320d9c63..49c686a09 100644 --- a/include/fcl/math/motion/taylor_model/time_interval.h +++ b/include/fcl/math/motion/taylor_model/time_interval.h @@ -47,7 +47,7 @@ namespace fcl { template -struct FCL_EXPORT TimeInterval +struct TimeInterval { /// @brief time Interval and different powers Interval t_; // [t1, t2] diff --git a/include/fcl/math/motion/tbv_motion_bound_visitor.h b/include/fcl/math/motion/tbv_motion_bound_visitor.h index 515fef80e..05c947c42 100644 --- a/include/fcl/math/motion/tbv_motion_bound_visitor.h +++ b/include/fcl/math/motion/tbv_motion_bound_visitor.h @@ -62,7 +62,7 @@ template class TranslationMotion; template -class FCL_EXPORT TBVMotionBoundVisitor : public BVMotionBoundVisitor +class TBVMotionBoundVisitor : public BVMotionBoundVisitor { public: using S = typename BV::S; diff --git a/include/fcl/math/motion/translation_motion-inl.h b/include/fcl/math/motion/translation_motion-inl.h index a2aaf6caf..ceb350094 100644 --- a/include/fcl/math/motion/translation_motion-inl.h +++ b/include/fcl/math/motion/translation_motion-inl.h @@ -47,7 +47,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT TranslationMotion; +class FCL_EXTERN_TEMPLATE_API TranslationMotion; //============================================================================== template diff --git a/include/fcl/math/motion/translation_motion.h b/include/fcl/math/motion/translation_motion.h index 37b77b470..ff2a1ff19 100644 --- a/include/fcl/math/motion/translation_motion.h +++ b/include/fcl/math/motion/translation_motion.h @@ -46,7 +46,7 @@ namespace fcl { template -class FCL_EXPORT TranslationMotion : public MotionBase +class TranslationMotion : public MotionBase { public: /// @brief Construct motion from intial and goal transform diff --git a/include/fcl/math/motion/triangle_motion_bound_visitor-inl.h b/include/fcl/math/motion/triangle_motion_bound_visitor-inl.h index 59f37cb03..29212d1a8 100644 --- a/include/fcl/math/motion/triangle_motion_bound_visitor-inl.h +++ b/include/fcl/math/motion/triangle_motion_bound_visitor-inl.h @@ -51,7 +51,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT TriangleMotionBoundVisitor; +class FCL_EXTERN_TEMPLATE_API TriangleMotionBoundVisitor; //============================================================================== template diff --git a/include/fcl/math/motion/triangle_motion_bound_visitor.h b/include/fcl/math/motion/triangle_motion_bound_visitor.h index e6a3b7d28..c37a206a7 100644 --- a/include/fcl/math/motion/triangle_motion_bound_visitor.h +++ b/include/fcl/math/motion/triangle_motion_bound_visitor.h @@ -71,7 +71,7 @@ template struct TriangleMotionBoundVisitorVisitImpl; template -class FCL_EXPORT TriangleMotionBoundVisitor +class TriangleMotionBoundVisitor { public: TriangleMotionBoundVisitor( diff --git a/include/fcl/math/rng-inl.h b/include/fcl/math/rng-inl.h index 1ba9da77b..687953a6d 100644 --- a/include/fcl/math/rng-inl.h +++ b/include/fcl/math/rng-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT RNG; +class FCL_EXTERN_TEMPLATE_API RNG; //============================================================================== template diff --git a/include/fcl/math/rng.h b/include/fcl/math/rng.h index 782cf48ce..827c0ea6d 100644 --- a/include/fcl/math/rng.h +++ b/include/fcl/math/rng.h @@ -55,7 +55,7 @@ namespace fcl /// threads. It is also guaranteed that all created instances will /// have a different random seed. template -class FCL_EXPORT RNG +class RNG { public: /// @brief Constructor. Always sets a different random seed diff --git a/include/fcl/math/sampler/sampler_base.h b/include/fcl/math/sampler/sampler_base.h index d49d8c99f..3150971ce 100644 --- a/include/fcl/math/sampler/sampler_base.h +++ b/include/fcl/math/sampler/sampler_base.h @@ -44,14 +44,14 @@ namespace fcl { template -class FCL_EXPORT SamplerBase +class SamplerBase { public: mutable RNG rng; }; extern template -class FCL_EXPORT SamplerBase; +class FCL_EXTERN_TEMPLATE_API SamplerBase; } // namespace fcl diff --git a/include/fcl/math/sampler/sampler_r.h b/include/fcl/math/sampler/sampler_r.h index 9b1977624..44eed4678 100644 --- a/include/fcl/math/sampler/sampler_r.h +++ b/include/fcl/math/sampler/sampler_r.h @@ -46,7 +46,7 @@ namespace fcl { template -class FCL_EXPORT SamplerR : public SamplerBase +class SamplerR : public SamplerBase { public: SamplerR(); diff --git a/include/fcl/math/sampler/sampler_se2-inl.h b/include/fcl/math/sampler/sampler_se2-inl.h index aa3cb9a84..78428d4ca 100644 --- a/include/fcl/math/sampler/sampler_se2-inl.h +++ b/include/fcl/math/sampler/sampler_se2-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SamplerSE2; +class FCL_EXTERN_TEMPLATE_API SamplerSE2; //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se2.h b/include/fcl/math/sampler/sampler_se2.h index ff321e71c..be626a522 100644 --- a/include/fcl/math/sampler/sampler_se2.h +++ b/include/fcl/math/sampler/sampler_se2.h @@ -45,7 +45,7 @@ namespace fcl { template -class FCL_EXPORT SamplerSE2 : public SamplerBase +class SamplerSE2 : public SamplerBase { public: SamplerSE2(); diff --git a/include/fcl/math/sampler/sampler_se2_disk-inl.h b/include/fcl/math/sampler/sampler_se2_disk-inl.h index 64aa25622..6575e828b 100644 --- a/include/fcl/math/sampler/sampler_se2_disk-inl.h +++ b/include/fcl/math/sampler/sampler_se2_disk-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SamplerSE2_disk; +class FCL_EXTERN_TEMPLATE_API SamplerSE2_disk; //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se2_disk.h b/include/fcl/math/sampler/sampler_se2_disk.h index 154110b7e..eed432d0f 100644 --- a/include/fcl/math/sampler/sampler_se2_disk.h +++ b/include/fcl/math/sampler/sampler_se2_disk.h @@ -45,7 +45,7 @@ namespace fcl { template -class FCL_EXPORT SamplerSE2_disk : public SamplerBase +class SamplerSE2_disk : public SamplerBase { public: SamplerSE2_disk(); diff --git a/include/fcl/math/sampler/sampler_se3_euler-inl.h b/include/fcl/math/sampler/sampler_se3_euler-inl.h index 14ff1b884..4ece176f6 100644 --- a/include/fcl/math/sampler/sampler_se3_euler-inl.h +++ b/include/fcl/math/sampler/sampler_se3_euler-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SamplerSE3Euler; +class FCL_EXTERN_TEMPLATE_API SamplerSE3Euler; //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se3_euler.h b/include/fcl/math/sampler/sampler_se3_euler.h index 487e22a9b..5ec8d9228 100644 --- a/include/fcl/math/sampler/sampler_se3_euler.h +++ b/include/fcl/math/sampler/sampler_se3_euler.h @@ -45,7 +45,7 @@ namespace fcl { template -class FCL_EXPORT SamplerSE3Euler : public SamplerBase +class SamplerSE3Euler : public SamplerBase { public: SamplerSE3Euler(); diff --git a/include/fcl/math/sampler/sampler_se3_euler_ball-inl.h b/include/fcl/math/sampler/sampler_se3_euler_ball-inl.h index 48dc6d972..69e9169a4 100644 --- a/include/fcl/math/sampler/sampler_se3_euler_ball-inl.h +++ b/include/fcl/math/sampler/sampler_se3_euler_ball-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SamplerSE3Euler_ball; +class FCL_EXTERN_TEMPLATE_API SamplerSE3Euler_ball; //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se3_euler_ball.h b/include/fcl/math/sampler/sampler_se3_euler_ball.h index 616e0b7c0..610908a42 100644 --- a/include/fcl/math/sampler/sampler_se3_euler_ball.h +++ b/include/fcl/math/sampler/sampler_se3_euler_ball.h @@ -45,7 +45,7 @@ namespace fcl { template -class FCL_EXPORT SamplerSE3Euler_ball : public SamplerBase +class SamplerSE3Euler_ball : public SamplerBase { public: SamplerSE3Euler_ball(); @@ -53,7 +53,7 @@ class FCL_EXPORT SamplerSE3Euler_ball : public SamplerBase SamplerSE3Euler_ball(S r_); void setBound(const S& r_); - + void getBound(S& r_) const; Vector6 sample() const; diff --git a/include/fcl/math/sampler/sampler_se3_quat-inl.h b/include/fcl/math/sampler/sampler_se3_quat-inl.h index ed961abb3..dba3e0be0 100644 --- a/include/fcl/math/sampler/sampler_se3_quat-inl.h +++ b/include/fcl/math/sampler/sampler_se3_quat-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SamplerSE3Quat; +class FCL_EXTERN_TEMPLATE_API SamplerSE3Quat; //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se3_quat.h b/include/fcl/math/sampler/sampler_se3_quat.h index 3c2ae989f..0f0a9b4f2 100644 --- a/include/fcl/math/sampler/sampler_se3_quat.h +++ b/include/fcl/math/sampler/sampler_se3_quat.h @@ -45,7 +45,7 @@ namespace fcl { template -class FCL_EXPORT SamplerSE3Quat : public SamplerBase +class SamplerSE3Quat : public SamplerBase { public: SamplerSE3Quat(); diff --git a/include/fcl/math/sampler/sampler_se3_quat_ball-inl.h b/include/fcl/math/sampler/sampler_se3_quat_ball-inl.h index 6a13b91ff..2fe34bccb 100644 --- a/include/fcl/math/sampler/sampler_se3_quat_ball-inl.h +++ b/include/fcl/math/sampler/sampler_se3_quat_ball-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT SamplerSE3Quat_ball; +class FCL_EXTERN_TEMPLATE_API SamplerSE3Quat_ball; //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se3_quat_ball.h b/include/fcl/math/sampler/sampler_se3_quat_ball.h index 9fef44c4e..42a527de0 100644 --- a/include/fcl/math/sampler/sampler_se3_quat_ball.h +++ b/include/fcl/math/sampler/sampler_se3_quat_ball.h @@ -45,7 +45,7 @@ namespace fcl { template -class FCL_EXPORT SamplerSE3Quat_ball : public SamplerBase +class SamplerSE3Quat_ball : public SamplerBase { public: SamplerSE3Quat_ball(); diff --git a/include/fcl/math/triangle.h b/include/fcl/math/triangle.h index f7ce26a5f..a639a606d 100644 --- a/include/fcl/math/triangle.h +++ b/include/fcl/math/triangle.h @@ -45,7 +45,7 @@ namespace fcl { /// @brief Triangle with 3 indices for points -class FCL_EXPORT Triangle +class FCL_API Triangle { /// @brief indices for each vertex of triangle std::size_t vids[3]; diff --git a/include/fcl/math/variance3-inl.h b/include/fcl/math/variance3-inl.h index dea534005..a0e61cd46 100644 --- a/include/fcl/math/variance3-inl.h +++ b/include/fcl/math/variance3-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT Variance3; +class FCL_EXTERN_TEMPLATE_API Variance3; //============================================================================== template diff --git a/include/fcl/math/variance3.h b/include/fcl/math/variance3.h index b5938206b..0bdb905bd 100644 --- a/include/fcl/math/variance3.h +++ b/include/fcl/math/variance3.h @@ -48,7 +48,7 @@ namespace fcl /// @brief Class for variance matrix in 3d template -class FCL_EXPORT Variance3 +class Variance3 { public: /// @brief Variation matrix diff --git a/include/fcl/narrowphase/collision-inl.h b/include/fcl/narrowphase/collision-inl.h index 81dc22d5f..314278031 100644 --- a/include/fcl/narrowphase/collision-inl.h +++ b/include/fcl/narrowphase/collision-inl.h @@ -49,7 +49,7 @@ namespace fcl //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API std::size_t collide( const CollisionObject* o1, const CollisionObject* o2, @@ -58,7 +58,7 @@ std::size_t collide( //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API std::size_t collide( const CollisionGeometry* o1, const Transform3& tf1, @@ -77,7 +77,6 @@ detail::CollisionFunctionMatrix& getCollisionFunctionLookTable() //============================================================================== template -FCL_EXPORT std::size_t collide( const CollisionObject* o1, const CollisionObject* o2, @@ -91,7 +90,6 @@ std::size_t collide( //============================================================================== template -FCL_EXPORT std::size_t collide( const CollisionGeometry* o1, const Transform3& tf1, @@ -150,7 +148,6 @@ std::size_t collide( //============================================================================== template -FCL_EXPORT std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const CollisionRequest& request, CollisionResult& result) { @@ -176,7 +173,6 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, //============================================================================== template -FCL_EXPORT std::size_t collide( const CollisionGeometry* o1, const Transform3& tf1, diff --git a/include/fcl/narrowphase/collision.h b/include/fcl/narrowphase/collision.h index f6e361e19..960e6246b 100644 --- a/include/fcl/narrowphase/collision.h +++ b/include/fcl/narrowphase/collision.h @@ -53,13 +53,11 @@ namespace fcl /// performs the collision between them. Return value is the number of contacts /// generated between the two objects. template -FCL_EXPORT std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, const CollisionRequest& request, CollisionResult& result); template -FCL_EXPORT std::size_t collide(const CollisionGeometry* o1, const Transform3& tf1, const CollisionGeometry* o2, const Transform3& tf2, const CollisionRequest& request, diff --git a/include/fcl/narrowphase/collision_object-inl.h b/include/fcl/narrowphase/collision_object-inl.h index 0c50e52c8..54d7b9502 100644 --- a/include/fcl/narrowphase/collision_object-inl.h +++ b/include/fcl/narrowphase/collision_object-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT CollisionObject; +class FCL_EXTERN_TEMPLATE_API CollisionObject; //============================================================================== template diff --git a/include/fcl/narrowphase/collision_object.h b/include/fcl/narrowphase/collision_object.h index bef211a49..120bcbc23 100644 --- a/include/fcl/narrowphase/collision_object.h +++ b/include/fcl/narrowphase/collision_object.h @@ -48,7 +48,7 @@ namespace fcl /// @brief the object for collision or distance computation, contains the /// geometry and the transform information template -class FCL_EXPORT CollisionObject +class CollisionObject { public: CollisionObject(const std::shared_ptr>& cgeom); diff --git a/include/fcl/narrowphase/collision_request-inl.h b/include/fcl/narrowphase/collision_request-inl.h index eb779014a..3aa7a21b2 100644 --- a/include/fcl/narrowphase/collision_request-inl.h +++ b/include/fcl/narrowphase/collision_request-inl.h @@ -47,7 +47,7 @@ namespace fcl //============================================================================== extern template -struct CollisionRequest; +struct FCL_EXTERN_TEMPLATE_API CollisionRequest; //============================================================================== template diff --git a/include/fcl/narrowphase/collision_request.h b/include/fcl/narrowphase/collision_request.h index 33417141c..fa85ac153 100644 --- a/include/fcl/narrowphase/collision_request.h +++ b/include/fcl/narrowphase/collision_request.h @@ -49,7 +49,7 @@ struct CollisionResult; /// @brief Parameters for performing collision request. template -struct FCL_EXPORT CollisionRequest +struct CollisionRequest { /// The underlying numerical representation of the request's scalar (e.g., /// float or double). @@ -81,7 +81,7 @@ struct FCL_EXPORT CollisionRequest // single std::optional>. /// @brief If true, uses the provided initial guess for the GJK algorithm. bool enable_cached_gjk_guess; - + /// @brief The initial guess to use in the GJK algorithm. Vector3 cached_gjk_guess; diff --git a/include/fcl/narrowphase/collision_result-inl.h b/include/fcl/narrowphase/collision_result-inl.h index e8829be23..9b52aa2f9 100644 --- a/include/fcl/narrowphase/collision_result-inl.h +++ b/include/fcl/narrowphase/collision_result-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -struct CollisionResult; +struct FCL_EXTERN_TEMPLATE_API CollisionResult; //============================================================================== template diff --git a/include/fcl/narrowphase/collision_result.h b/include/fcl/narrowphase/collision_result.h index a0d367773..b7f36312c 100644 --- a/include/fcl/narrowphase/collision_result.h +++ b/include/fcl/narrowphase/collision_result.h @@ -49,7 +49,7 @@ namespace fcl /// @brief collision result template -struct FCL_EXPORT CollisionResult +struct CollisionResult { private: /// @brief contact information @@ -85,7 +85,7 @@ struct FCL_EXPORT CollisionResult /// @brief get all the contacts void getContacts(std::vector>& contacts_); - /// @brief get all the cost sources + /// @brief get all the cost sources void getCostSources(std::vector>& cost_sources_); /// @brief clear the results obtained diff --git a/include/fcl/narrowphase/contact-inl.h b/include/fcl/narrowphase/contact-inl.h index 88d26e557..00fb43c02 100644 --- a/include/fcl/narrowphase/contact-inl.h +++ b/include/fcl/narrowphase/contact-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -struct Contact; +struct FCL_EXTERN_TEMPLATE_API Contact; //============================================================================== template diff --git a/include/fcl/narrowphase/contact.h b/include/fcl/narrowphase/contact.h index 0994036e4..a726cb919 100644 --- a/include/fcl/narrowphase/contact.h +++ b/include/fcl/narrowphase/contact.h @@ -45,7 +45,7 @@ namespace fcl /// @brief Contact information returned by collision template -struct FCL_EXPORT Contact +struct Contact { /// @brief collision object 1 const CollisionGeometry* o1; @@ -64,7 +64,7 @@ struct FCL_EXPORT Contact /// if object 2 is geometry shape, it is NONE (-1), /// if object 2 is octree, it is the id of the cell int b2; - + /// @brief contact normal, pointing from o1 to o2 Vector3 normal; @@ -74,7 +74,7 @@ struct FCL_EXPORT Contact /// @brief penetration depth S penetration_depth; - + /// @brief invalid contact primitive information static const int NONE = -1; diff --git a/include/fcl/narrowphase/contact_point-inl.h b/include/fcl/narrowphase/contact_point-inl.h index 74ab8ca0d..d6fe17b5f 100644 --- a/include/fcl/narrowphase/contact_point-inl.h +++ b/include/fcl/narrowphase/contact_point-inl.h @@ -45,15 +45,17 @@ namespace fcl //============================================================================== extern template -struct ContactPoint; +struct FCL_EXTERN_TEMPLATE_API ContactPoint; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool comparePenDepth( const ContactPoint& _cp1, const ContactPoint& _cp2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void flipNormal(std::vector>& contacts); //============================================================================== diff --git a/include/fcl/narrowphase/contact_point.h b/include/fcl/narrowphase/contact_point.h index bca98e6bf..c917d6987 100644 --- a/include/fcl/narrowphase/contact_point.h +++ b/include/fcl/narrowphase/contact_point.h @@ -45,7 +45,7 @@ namespace fcl /// @brief Minimal contact information returned by collision template -struct FCL_EXPORT ContactPoint +struct ContactPoint { /// @brief Contact normal, pointing from o1 to o2 Vector3 normal; @@ -68,12 +68,10 @@ using ContactPointd = ContactPoint; /// @brief Return true if _cp1's penentration depth is less than _cp2's. template -FCL_EXPORT bool comparePenDepth( const ContactPoint& _cp1, const ContactPoint& _cp2); template -FCL_EXPORT void flipNormal(std::vector>& contacts); } // namespace fcl diff --git a/include/fcl/narrowphase/continuous_collision-inl.h b/include/fcl/narrowphase/continuous_collision-inl.h index 1adfb6ec2..d88c5927e 100644 --- a/include/fcl/narrowphase/continuous_collision-inl.h +++ b/include/fcl/narrowphase/continuous_collision-inl.h @@ -56,6 +56,7 @@ namespace fcl //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double continuousCollide( const CollisionGeometry* o1, const MotionBase* motion1, @@ -66,6 +67,7 @@ double continuousCollide( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double continuousCollide( const CollisionGeometry* o1, const Transform3& tf1_beg, @@ -78,6 +80,7 @@ double continuousCollide( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double continuousCollide( const CollisionObject* o1, const Transform3& tf1_end, @@ -88,6 +91,7 @@ double continuousCollide( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double collide( const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, @@ -105,7 +109,6 @@ getConservativeAdvancementFunctionLookTable() //============================================================================== template -FCL_EXPORT MotionBasePtr getMotionBase( const Transform3& tf_beg, const Transform3& tf_end, @@ -132,7 +135,6 @@ MotionBasePtr getMotionBase( //============================================================================== template -FCL_EXPORT S continuousCollideNaive( const CollisionGeometry* o1, const MotionBase* motion1, @@ -175,7 +177,6 @@ namespace detail //============================================================================== template -FCL_EXPORT typename BV::S continuousCollideBVHPolynomial( const CollisionGeometry* o1_, const TranslationMotion* motion1, @@ -244,7 +245,6 @@ typename BV::S continuousCollideBVHPolynomial( //============================================================================== template -FCL_EXPORT S continuousCollideBVHPolynomial( const CollisionGeometry* o1, const TranslationMotion* motion1, @@ -301,7 +301,6 @@ namespace detail //============================================================================== template -FCL_EXPORT typename NarrowPhaseSolver::S continuousCollideConservativeAdvancement( const CollisionGeometry* o1, const MotionBase* motion1, @@ -355,7 +354,6 @@ typename NarrowPhaseSolver::S continuousCollideConservativeAdvancement( } // namespace detail template -FCL_EXPORT S continuousCollideConservativeAdvancement( const CollisionGeometry* o1, const MotionBase* motion1, @@ -383,7 +381,6 @@ S continuousCollideConservativeAdvancement( //============================================================================== template -FCL_EXPORT S continuousCollide( const CollisionGeometry* o1, const MotionBase* motion1, @@ -433,7 +430,6 @@ S continuousCollide( //============================================================================== template -FCL_EXPORT S continuousCollide( const CollisionGeometry* o1, const Transform3& tf1_beg, @@ -452,7 +448,6 @@ S continuousCollide( //============================================================================== template -FCL_EXPORT S continuousCollide( const CollisionObject* o1, const Transform3& tf1_end, @@ -468,7 +463,6 @@ S continuousCollide( //============================================================================== template -FCL_EXPORT S collide( const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, diff --git a/include/fcl/narrowphase/continuous_collision.h b/include/fcl/narrowphase/continuous_collision.h index fb1cab6c3..fe02b38e4 100644 --- a/include/fcl/narrowphase/continuous_collision.h +++ b/include/fcl/narrowphase/continuous_collision.h @@ -51,7 +51,6 @@ namespace fcl /// @brief continous collision checking between two objects template -FCL_EXPORT S continuousCollide( const CollisionGeometry* o1, const MotionBase* motion1, @@ -61,7 +60,6 @@ S continuousCollide( ContinuousCollisionResult& result); template -FCL_EXPORT S continuousCollide( const CollisionGeometry* o1, const Transform3& tf1_beg, @@ -73,7 +71,6 @@ S continuousCollide( ContinuousCollisionResult& result); template -FCL_EXPORT S continuousCollide( const CollisionObject* o1, const Transform3& tf1_end, @@ -83,7 +80,6 @@ S continuousCollide( ContinuousCollisionResult& result); template -FCL_EXPORT S collide( const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, diff --git a/include/fcl/narrowphase/continuous_collision_object-inl.h b/include/fcl/narrowphase/continuous_collision_object-inl.h index e8008e98e..0b39838b3 100644 --- a/include/fcl/narrowphase/continuous_collision_object-inl.h +++ b/include/fcl/narrowphase/continuous_collision_object-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -class FCL_EXPORT ContinuousCollisionObject; +class FCL_EXTERN_TEMPLATE_API ContinuousCollisionObject; //============================================================================== template diff --git a/include/fcl/narrowphase/continuous_collision_object.h b/include/fcl/narrowphase/continuous_collision_object.h index ecb12c422..60b874186 100644 --- a/include/fcl/narrowphase/continuous_collision_object.h +++ b/include/fcl/narrowphase/continuous_collision_object.h @@ -48,7 +48,7 @@ namespace fcl /// @brief the object for continuous collision or distance computation, contains /// the geometry and the motion information template -class FCL_EXPORT ContinuousCollisionObject +class ContinuousCollisionObject { public: ContinuousCollisionObject( diff --git a/include/fcl/narrowphase/continuous_collision_request-inl.h b/include/fcl/narrowphase/continuous_collision_request-inl.h index cd08e7fc7..5096710c8 100644 --- a/include/fcl/narrowphase/continuous_collision_request-inl.h +++ b/include/fcl/narrowphase/continuous_collision_request-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -struct ContinuousCollisionRequest; +struct FCL_EXTERN_TEMPLATE_API ContinuousCollisionRequest; //============================================================================== template diff --git a/include/fcl/narrowphase/continuous_collision_request.h b/include/fcl/narrowphase/continuous_collision_request.h index 356be14cd..f1a0f67ad 100644 --- a/include/fcl/narrowphase/continuous_collision_request.h +++ b/include/fcl/narrowphase/continuous_collision_request.h @@ -49,7 +49,7 @@ enum CCDMotionType {CCDM_TRANS, CCDM_LINEAR, CCDM_SCREW, CCDM_SPLINE}; enum CCDSolverType {CCDC_NAIVE, CCDC_CONSERVATIVE_ADVANCEMENT, CCDC_RAY_SHOOTING, CCDC_POLYNOMIAL_SOLVER}; template -struct FCL_EXPORT ContinuousCollisionRequest +struct ContinuousCollisionRequest { /// @brief maximum num of iterations std::size_t num_max_iterations; @@ -65,13 +65,13 @@ struct FCL_EXPORT ContinuousCollisionRequest /// @brief ccd solver type CCDSolverType ccd_solver_type; - + ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10, S toc_err_ = 0.0001, CCDMotionType ccd_motion_type_ = CCDM_TRANS, GJKSolverType gjk_solver_type_ = GST_LIBCCD, CCDSolverType ccd_solver_type_ = CCDC_NAIVE); - + }; using ContinuousCollisionRequestf = ContinuousCollisionRequest; diff --git a/include/fcl/narrowphase/continuous_collision_result-inl.h b/include/fcl/narrowphase/continuous_collision_result-inl.h index a34d168c0..ff0416002 100644 --- a/include/fcl/narrowphase/continuous_collision_result-inl.h +++ b/include/fcl/narrowphase/continuous_collision_result-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -struct ContinuousCollisionResult; +struct FCL_EXTERN_TEMPLATE_API ContinuousCollisionResult; //============================================================================== template diff --git a/include/fcl/narrowphase/continuous_collision_result.h b/include/fcl/narrowphase/continuous_collision_result.h index e19bf6941..473a856ec 100644 --- a/include/fcl/narrowphase/continuous_collision_result.h +++ b/include/fcl/narrowphase/continuous_collision_result.h @@ -45,18 +45,18 @@ namespace fcl /// @brief continuous collision result template -struct FCL_EXPORT ContinuousCollisionResult +struct ContinuousCollisionResult { /// @brief collision or not bool is_collide; - + /// @brief time of contact in [0, 1] S time_of_contact; Transform3 contact_tf1; Transform3 contact_tf2; - + ContinuousCollisionResult(); EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/include/fcl/narrowphase/cost_source-inl.h b/include/fcl/narrowphase/cost_source-inl.h index abadfc960..31b90bacf 100644 --- a/include/fcl/narrowphase/cost_source-inl.h +++ b/include/fcl/narrowphase/cost_source-inl.h @@ -45,7 +45,7 @@ namespace fcl //============================================================================== extern template -struct CostSource; +struct FCL_EXTERN_TEMPLATE_API CostSource; //============================================================================== template diff --git a/include/fcl/narrowphase/cost_source.h b/include/fcl/narrowphase/cost_source.h index 15bc11180..66f072a18 100644 --- a/include/fcl/narrowphase/cost_source.h +++ b/include/fcl/narrowphase/cost_source.h @@ -46,7 +46,7 @@ namespace fcl /// @brief Cost source describes an area with a cost. The area is described by an AABB region. template -struct FCL_EXPORT CostSource +struct CostSource { /// @brief aabb lower bound Vector3 aabb_min; diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa-inl.h index e9c667d6d..d1b2b648c 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa-inl.h @@ -48,7 +48,7 @@ namespace detail //============================================================================== extern template -struct EPA; +struct FCL_EXTERN_TEMPLATE_API EPA; //============================================================================== template diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h index eb7efa4b1..8d81137f8 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h @@ -54,7 +54,7 @@ namespace detail /// @brief class for EPA algorithm template -struct FCL_EXPORT EPA +struct EPA { private: using SimplexV = typename GJK::SimplexV; @@ -101,7 +101,7 @@ struct FCL_EXPORT EPA public: enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed}; - + Status status; typename GJK::Simplex result; Vector3 normal; @@ -130,8 +130,8 @@ struct FCL_EXPORT EPA Status evaluate(GJK& gjk, const Vector3& guess); - /// @brief the goal is to add a face connecting vertex w and face edge f[e] - bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); + /// @brief the goal is to add a face connecting vertex w and face edge f[e] + bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); }; using EPAf = EPA; diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk-inl.h index fa01dc352..768ea39e3 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk-inl.h @@ -48,7 +48,7 @@ namespace detail //============================================================================== extern template -struct GJK; +struct FCL_EXTERN_TEMPLATE_API GJK; //============================================================================== template diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h index f5bbc2107..5fb481ffc 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h @@ -49,7 +49,7 @@ namespace detail /// @brief class for GJK algorithm template -struct FCL_EXPORT GJK +struct GJK { struct SimplexV { @@ -63,7 +63,7 @@ struct FCL_EXPORT GJK { /// @brief simplex vertex SimplexV* c[4]; - /// @brief weight + /// @brief weight S p[4]; /// @brief size of simplex (number of vertices) size_t rank; @@ -79,7 +79,7 @@ struct FCL_EXPORT GJK Simplex simplices[2]; GJK(unsigned int max_iterations_, S tolerance_); - + void initialize(); /// @brief GJK algorithm, given the initial value guess 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 98368bfb0..a7e42ff99 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 @@ -56,39 +56,41 @@ namespace detail //============================================================================== extern template -class FCL_EXPORT GJKInitializer>; +class FCL_EXTERN_TEMPLATE_API GJKInitializer>; //============================================================================== extern template -class FCL_EXPORT GJKInitializer>; +class FCL_EXTERN_TEMPLATE_API GJKInitializer>; //============================================================================== extern template -class FCL_EXPORT GJKInitializer>; +class FCL_EXTERN_TEMPLATE_API GJKInitializer>; //============================================================================== extern template -class FCL_EXPORT GJKInitializer>; +class FCL_EXTERN_TEMPLATE_API GJKInitializer>; //============================================================================== extern template -class FCL_EXPORT GJKInitializer>; +class FCL_EXTERN_TEMPLATE_API GJKInitializer>; //============================================================================== extern template -class FCL_EXPORT GJKInitializer>; +class FCL_EXTERN_TEMPLATE_API GJKInitializer>; //============================================================================== extern template -class FCL_EXPORT GJKInitializer>; +class FCL_EXTERN_TEMPLATE_API GJKInitializer>; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void* triCreateGJKObject( const Vector3d& P1, const Vector3d& P2, const Vector3d& P3); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void* triCreateGJKObject( const Vector3d& P1, const Vector3d& P2, @@ -97,6 +99,7 @@ void* triCreateGJKObject( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool GJKCollide( void* obj1, ccd_support_fn supp1, @@ -112,6 +115,7 @@ bool GJKCollide( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool GJKDistance( void* obj1, ccd_support_fn supp1, @@ -124,6 +128,7 @@ bool GJKDistance( Vector3d* p2); extern template +FCL_EXTERN_TEMPLATE_API bool GJKSignedDistance( void* obj1, ccd_support_fn supp1, diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h index 291772960..d7388ac57 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h @@ -74,7 +74,7 @@ using GJKCenterFunction = void (*)(const void* obj, ccd_vec3_t* c); /// @brief initialize GJK stuffs template -class FCL_EXPORT GJKInitializer +class GJKInitializer { public: /// @brief Get GJK support function @@ -94,7 +94,7 @@ class FCL_EXPORT GJKInitializer /// @brief initialize GJK Cylinder template -class FCL_EXPORT GJKInitializer> +class GJKInitializer> { public: static GJKSupportFunction getSupportFunction(); @@ -105,7 +105,7 @@ class FCL_EXPORT GJKInitializer> /// @brief initialize GJK Sphere template -class FCL_EXPORT GJKInitializer> +class GJKInitializer> { public: static GJKSupportFunction getSupportFunction(); @@ -116,7 +116,7 @@ class FCL_EXPORT GJKInitializer> /// @brief initialize GJK Ellipsoid template -class FCL_EXPORT GJKInitializer> +class GJKInitializer> { public: static GJKSupportFunction getSupportFunction(); @@ -127,7 +127,7 @@ class FCL_EXPORT GJKInitializer> /// @brief initialize GJK Box template -class FCL_EXPORT GJKInitializer> +class GJKInitializer> { public: static GJKSupportFunction getSupportFunction(); @@ -138,7 +138,7 @@ class FCL_EXPORT GJKInitializer> /// @brief initialize GJK Capsule template -class FCL_EXPORT GJKInitializer> +class GJKInitializer> { public: static GJKSupportFunction getSupportFunction(); @@ -149,7 +149,7 @@ class FCL_EXPORT GJKInitializer> /// @brief initialize GJK Cone template -class FCL_EXPORT GJKInitializer> +class GJKInitializer> { public: static GJKSupportFunction getSupportFunction(); @@ -160,7 +160,7 @@ class FCL_EXPORT GJKInitializer> /// @brief initialize GJK Convex template -class FCL_EXPORT GJKInitializer> +class GJKInitializer> { public: static GJKSupportFunction getSupportFunction(); @@ -170,26 +170,23 @@ class FCL_EXPORT GJKInitializer> }; /// @brief initialize GJK Triangle -FCL_EXPORT +FCL_API GJKSupportFunction triGetSupportFunction(); -FCL_EXPORT +FCL_API GJKCenterFunction triGetCenterFunction(); template -FCL_EXPORT void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vector3& P3); template -FCL_EXPORT void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf); -FCL_EXPORT +FCL_API void triDeleteGJKObject(void* o); /// @brief GJK collision algorithm template -FCL_EXPORT bool GJKCollide( void* obj1, ccd_support_fn supp1, @@ -222,7 +219,6 @@ bool GJKCollide( * @retval is_separated True if the objects are separated, false otherwise. */ template -FCL_EXPORT bool GJKDistance(void* obj1, ccd_support_fn supp1, void* obj2, ccd_support_fn supp2, unsigned int max_iterations, S tolerance, @@ -248,7 +244,6 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1, * @retval is_separated True if the objects are separated, false otherwise. */ template -FCL_EXPORT bool GJKSignedDistance(void* obj1, ccd_support_fn supp1, void* obj2, ccd_support_fn supp2, unsigned int max_iterations, S tolerance, diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h index 9f1309e06..abceb5704 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h @@ -59,11 +59,10 @@ namespace detail //============================================================================== extern template -struct MinkowskiDiff; +struct FCL_EXTERN_TEMPLATE_API MinkowskiDiff; //============================================================================== template -FCL_EXPORT Vector3 getSupport( const ShapeBase* shape, const Eigen::MatrixBase& dir) diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h index a1ee9c7da..d3f967d07 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h @@ -55,7 +55,7 @@ Vector3 getSupport( /// @brief Minkowski difference class of two shapes template -struct FCL_EXPORT MinkowskiDiff +struct MinkowskiDiff { /// @brief points to two shapes const ShapeBase* shapes[2]; @@ -63,14 +63,14 @@ struct FCL_EXPORT MinkowskiDiff /// @brief rotation from shape0 to shape1 Matrix3 toshape1; - /// @brief transform from shape1 to shape0 + /// @brief transform from shape1 to shape0 Transform3 toshape0; MinkowskiDiff(); /// @brief support function for shape0 Vector3 support0(const Vector3& d) const; - + /// @brief support function for shape1 Vector3 support1(const Vector3& d) const; diff --git a/include/fcl/narrowphase/detail/failed_at_this_configuration.h b/include/fcl/narrowphase/detail/failed_at_this_configuration.h index b36bb5952..bef56934a 100644 --- a/include/fcl/narrowphase/detail/failed_at_this_configuration.h +++ b/include/fcl/narrowphase/detail/failed_at_this_configuration.h @@ -61,7 +61,7 @@ namespace detail { FCL_THROW_UNEXPECTED_CONFIGURATION_EXCEPTION defined below. Code that exercises functionality that throws this type of exception should catch it and transform it to a `std::logic_error` by invoking ThrowDetailedConfiguration(). */ -class FCL_EXPORT FailedAtThisConfiguration final +class FCL_API FailedAtThisConfiguration final : public std::exception { public: FailedAtThisConfiguration(const std::string& message) @@ -82,12 +82,12 @@ class FCL_EXPORT FailedAtThisConfiguration final @param func The name of the invoking function. @param file The name of the file associated with the exception. @param line The line number where the exception is thrown. */ -FCL_EXPORT void ThrowFailedAtThisConfiguration( +FCL_API void ThrowFailedAtThisConfiguration( const std::string& message, const char* func, const char* file, int line); /** Helper class for propagating a low-level exception upwards but with configuration-specific details appended. The parameters - + @param s1 The first shape in the query. @param X_FS1 The pose of the first shape in frame F. @param s2 The second shape in the query. diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index ef9f8b158..ea8ce975f 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -67,7 +67,7 @@ namespace detail //============================================================================== extern template -struct GJKSolver_indep; +struct FCL_EXTERN_TEMPLATE_API GJKSolver_indep; //============================================================================== template diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep.h b/include/fcl/narrowphase/detail/gjk_solver_indep.h index 14eb10ada..0f3fade32 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep.h @@ -51,7 +51,7 @@ namespace detail /// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet) template -struct FCL_EXPORT GJKSolver_indep +struct GJKSolver_indep { using S = S_; diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 3cef4d657..b5f10dc4d 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -64,7 +64,7 @@ namespace detail //============================================================================== extern template -struct GJKSolver_libccd; +struct FCL_EXTERN_TEMPLATE_API GJKSolver_libccd; //============================================================================== template diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd.h b/include/fcl/narrowphase/detail/gjk_solver_libccd.h index 218a89481..1b6b2c33d 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd.h @@ -51,7 +51,7 @@ namespace detail /// @brief collision and distance solver based on libccd library. template -struct FCL_EXPORT GJKSolver_libccd +struct GJKSolver_libccd { using S = S_; diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h index 91ffeca65..a9ea0cee1 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h @@ -50,20 +50,24 @@ namespace detail //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void lineClosestApproach(const Vector3& pa, const Vector3& ua, const Vector3& pb, const Vector3& ub, double* alpha, double* beta); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API int intersectRectQuad2(double h[2], double p[8], double ret[16]); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void cullPoints2(int n, double p[], int m, int i0, int iret[]); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API int boxBox2( const Vector3& side1, const Transform3& tf1, @@ -77,6 +81,7 @@ int boxBox2( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool boxBoxIntersect(const Box& s1, const Transform3& tf1, const Box& s2, const Transform3& tf2, std::vector>* contacts_); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h index 87a60845f..d7be73443 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box.h @@ -49,7 +49,6 @@ namespace detail { template -FCL_EXPORT void lineClosestApproach(const Vector3& pa, const Vector3& ua, const Vector3& pb, const Vector3& ub, S* alpha, S* beta); @@ -62,7 +61,6 @@ void lineClosestApproach(const Vector3& pa, const Vector3& ua, // the number of intersection points is returned by the function (this will // be in the range 0 to 8). template -FCL_EXPORT int intersectRectQuad2(S h[2], S p[8], S ret[16]); // given n points in the plane (array p, of size 2*n), generate m points that @@ -73,11 +71,9 @@ int intersectRectQuad2(S h[2], S p[8], S ret[16]); // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be // in the range [0..n-1]. template -FCL_EXPORT void cullPoints2(int n, S p[], int m, int i0, int iret[]); template -FCL_EXPORT int boxBox2( const Vector3& side1, const Eigen::MatrixBase& R1, @@ -92,7 +88,6 @@ int boxBox2( std::vector>& contacts); template -FCL_EXPORT int boxBox2( const Vector3& side1, const Transform3& tf1, @@ -105,7 +100,6 @@ int boxBox2( std::vector>& contacts); template -FCL_EXPORT bool boxBoxIntersect(const Box& s1, const Transform3& tf1, const Box& s2, const Transform3& tf2, std::vector>* contacts_); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h index b316c956d..dea64afc9 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h @@ -48,18 +48,22 @@ namespace detail //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double clamp(double n, double min, double max); //============================================================================== -extern template double closestPtSegmentSegment(const Vector3d& p_FP1, - const Vector3d& p_FQ1, - const Vector3d& p_FP2, - const Vector3d& p_FQ2, double* s, - double* t, Vector3d* p_FC1, - Vector3d* p_FC2); +extern template +FCL_EXTERN_TEMPLATE_API +double closestPtSegmentSegment(const Vector3d& p_FP1, + const Vector3d& p_FQ1, + const Vector3d& p_FP2, + const Vector3d& p_FQ2, double* s, + double* t, Vector3d* p_FC1, + Vector3d* p_FC2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool capsuleCapsuleDistance( const Capsule& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h index c3917368b..a1303fcaa 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h @@ -49,7 +49,6 @@ namespace detail // Clamp n to lie within the range [min, max] template -FCL_EXPORT S clamp(S n, S min, S max); /** Computes the pair of closest points `(p_FC1, p_FC2)` between two line @@ -73,11 +72,11 @@ S clamp(S n, S min, S max); @tparam S The scalar type for computation. */ template -FCL_EXPORT S closestPtSegmentSegment(const Vector3& p_FP1, - const Vector3& p_FQ1, - const Vector3& p_FP2, - const Vector3& p_FQ2, S* s, S* t, - Vector3* p_FC1, Vector3* p_FC2); +S closestPtSegmentSegment(const Vector3& p_FP1, + const Vector3& p_FQ1, + const Vector3& p_FP2, + const Vector3& p_FQ2, S* s, S* t, + Vector3* p_FC1, Vector3* p_FC2); /** Computes the signed distance between two capsules `s1` and `s2` (with given poses `X_FC1` and `X_FC2` of the two capsules in a common frame `F`). @@ -104,7 +103,6 @@ FCL_EXPORT S closestPtSegmentSegment(const Vector3& p_FP1, @tparam S The scalar type for computation. */ template -FCL_EXPORT bool capsuleCapsuleDistance(const Capsule& s1, const Transform3& X_FC1, const Capsule& s2, const Transform3& X_FC2, S* dist, Vector3* p_FW1, Vector3* p_FW2); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h index 70a9a872d..b4d0c8acb 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h @@ -48,6 +48,7 @@ namespace detail //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool sphereHalfspaceIntersect( const Sphere& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -55,6 +56,7 @@ bool sphereHalfspaceIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool ellipsoidHalfspaceIntersect( const Ellipsoid& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -62,12 +64,14 @@ bool ellipsoidHalfspaceIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool boxHalfspaceIntersect( const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool boxHalfspaceIntersect( const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -75,6 +79,7 @@ bool boxHalfspaceIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool capsuleHalfspaceIntersect( const Capsule& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -82,6 +87,7 @@ bool capsuleHalfspaceIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool cylinderHalfspaceIntersect( const Cylinder& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -89,6 +95,7 @@ bool cylinderHalfspaceIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool coneHalfspaceIntersect( const Cone& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -96,6 +103,7 @@ bool coneHalfspaceIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool convexHalfspaceIntersect( const Convex& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -103,6 +111,7 @@ bool convexHalfspaceIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool halfspaceTriangleIntersect( const Halfspace& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, @@ -110,6 +119,7 @@ bool halfspaceTriangleIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool planeHalfspaceIntersect( const Plane& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -120,6 +130,7 @@ bool planeHalfspaceIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool halfspacePlaneIntersect( const Halfspace& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, @@ -129,6 +140,7 @@ bool halfspacePlaneIntersect( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool halfspaceIntersect( const Halfspace& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h index c8339f76b..6b1d56fb4 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h @@ -56,25 +56,22 @@ namespace detail { template -FCL_EXPORT S halfspaceIntersectTolerance(); template <> -FCL_EXPORT +FCL_API float halfspaceIntersectTolerance(); template <> -FCL_EXPORT +FCL_API double halfspaceIntersectTolerance(); template -FCL_EXPORT bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool ellipsoidHalfspaceIntersect(const Ellipsoid& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, std::vector>* contacts); @@ -85,42 +82,35 @@ bool ellipsoidHalfspaceIntersect(const Ellipsoid& s1, const Transform3& tf /// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c /// the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough template -FCL_EXPORT bool boxHalfspaceIntersect(const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2); template -FCL_EXPORT bool boxHalfspaceIntersect(const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool coneHalfspaceIntersect(const Cone& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool convexHalfspaceIntersect(const Convex& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, Vector3* contact_points, S* penetration_depth, Vector3* normal); template -FCL_EXPORT bool halfspaceTriangleIntersect(const Halfspace& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, Vector3* contact_points, S* penetration_depth, Vector3* normal); @@ -133,7 +123,6 @@ bool halfspaceTriangleIntersect(const Halfspace& s1, const Transform3& tf1 /// if not parallel /// return the intersection ray, return code 3. ray origin is p and direction is d template -FCL_EXPORT bool planeHalfspaceIntersect(const Plane& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, Plane& pl, @@ -142,7 +131,6 @@ bool planeHalfspaceIntersect(const Plane& s1, const Transform3& tf1, int& ret); template -FCL_EXPORT bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, Plane& pl, Vector3& p, Vector3& d, @@ -158,7 +146,6 @@ bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3& tf1, /// if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d /// collision free return code 0 template -FCL_EXPORT bool halfspaceIntersect(const Halfspace& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, Vector3& p, Vector3& d, diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h index 1373dce1f..6b6d35f39 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h @@ -48,64 +48,75 @@ namespace detail //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool spherePlaneIntersect(const Sphere& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool boxPlaneIntersect(const Box& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool conePlaneIntersect(const Cone& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, Vector3* contact_points, double* penetration_depth, Vector3* normal); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool planeTriangleIntersect(const Plane& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, Vector3* contact_points, double* penetration_depth, Vector3* normal); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool planeIntersect(const Plane& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane.h index 86b141f24..073d50978 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane.h @@ -55,25 +55,22 @@ namespace detail { template -FCL_EXPORT S planeIntersectTolerance(); template <> -FCL_EXPORT +FCL_API double planeIntersectTolerance(); template <> -FCL_EXPORT +FCL_API float planeIntersectTolerance(); template -FCL_EXPORT bool spherePlaneIntersect(const Sphere& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); @@ -86,18 +83,15 @@ bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3& tf1, /// (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c. /// if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side. template -FCL_EXPORT bool boxPlaneIntersect(const Box& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); template -FCL_EXPORT bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); @@ -108,36 +102,30 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, /// (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0 /// (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0 template -FCL_EXPORT bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); template -FCL_EXPORT bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool conePlaneIntersect(const Cone& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, Vector3* contact_points, S* penetration_depth, Vector3* normal); template -FCL_EXPORT bool planeTriangleIntersect(const Plane& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, Vector3* contact_points, S* penetration_depth, Vector3* normal); template -FCL_EXPORT bool planeIntersect(const Plane& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h index 53cdace4e..4f4ba5bee 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h @@ -42,18 +42,20 @@ namespace fcl { namespace detail { -extern template FCL_EXPORT bool -sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, - const Box& box, const Transform3& X_FB, - std::vector>* contacts); +extern template +FCL_EXTERN_TEMPLATE_API +bool sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + std::vector>* contacts); //============================================================================== -extern template FCL_EXPORT bool -sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, - const Box& box, const Transform3& X_FB, - double* distance, Vector3* p_FSb, - Vector3* p_FBs); +extern template +FCL_EXTERN_TEMPLATE_API +bool sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + double* distance, Vector3* p_FSb, + Vector3* p_FBs); //============================================================================== @@ -95,10 +97,10 @@ bool nearestPointInBox(const Vector3& size, const Vector3& p_BQ, //============================================================================== template -FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, - const Transform3& X_FS, const Box& box, - const Transform3& X_FB, - std::vector>* contacts) { +bool sphereBoxIntersect(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, + std::vector>* contacts) { const S r = sphere.radius; // Find the sphere center C in the box's frame. const Transform3 X_BS = X_FB.inverse() * X_FS; @@ -180,10 +182,10 @@ FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, //============================================================================== template -FCL_EXPORT bool sphereBoxDistance(const Sphere& sphere, - const Transform3& X_FS, const Box& box, - const Transform3& X_FB, S* distance, - Vector3* p_FSb, Vector3* p_FBs) { +bool sphereBoxDistance(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, S* distance, + Vector3* p_FSb, Vector3* p_FBs) { // Find the sphere center C in the box's frame. const Transform3 X_BS = X_FB.inverse() * X_FS; const Vector3 p_BC = X_BS.translation(); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h index 87fabf8cd..478774b7b 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box.h @@ -107,10 +107,10 @@ namespace detail { @return True if the objects are colliding (including touching). @tparam S The scalar parameter (must be a valid Eigen scalar). */ template -FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, - const Transform3& X_FS, const Box& box, - const Transform3& X_FB, - std::vector>* contacts); +bool sphereBoxIntersect(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, + std::vector>* contacts); /** Evaluate the minimum separating distance between a sphere and box. If separated, the nearest points on each shape will be returned in frame F. @@ -127,10 +127,10 @@ FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, @return True if the objects are separated. @tparam S The scalar parameter (must be a valid Eigen scalar). */ template -FCL_EXPORT bool sphereBoxDistance(const Sphere& sphere, - const Transform3& X_FS, const Box& box, - const Transform3& X_FB, S* distance, - Vector3* p_FSb, Vector3* p_FBs); +bool sphereBoxDistance(const Sphere& sphere, + const Transform3& X_FS, const Box& box, + const Transform3& X_FB, S* distance, + Vector3* p_FSb, Vector3* p_FBs); //@} diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h index c85d56991..210a55135 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h @@ -48,6 +48,7 @@ namespace detail //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void lineSegmentPointClosestToPoint( const Vector3 &p, const Vector3 &s1, @@ -56,12 +57,14 @@ void lineSegmentPointClosestToPoint( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool sphereCapsuleIntersect(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h index 2820b32a7..81061f390 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.h @@ -53,7 +53,6 @@ namespace detail // given by Dan Sunday's page: // http://geomalgorithms.com/a02-_lines.html template -FCL_EXPORT void lineSegmentPointClosestToPoint( const Vector3 &p, const Vector3 &s1, @@ -61,13 +60,11 @@ void lineSegmentPointClosestToPoint( Vector3 &sp); template -FCL_EXPORT bool sphereCapsuleIntersect(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, std::vector>* contacts); template -FCL_EXPORT bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, S* dist, Vector3* p1, Vector3* p2); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h index 44070c04e..280962f2d 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h @@ -42,22 +42,24 @@ namespace fcl { namespace detail { -extern template FCL_EXPORT bool -sphereCylinderIntersect(const Sphere& sphere, - const Transform3& X_FS, - const Cylinder& cylinder, - const Transform3& X_FC, - std::vector>* contacts); +extern template +FCL_EXTERN_TEMPLATE_API +bool sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); //============================================================================== -extern template FCL_EXPORT bool -sphereCylinderDistance(const Sphere& sphere, - const Transform3& X_FS, - const Cylinder& cylinder, - const Transform3& X_FC, - double* distance, Vector3* p_FSc, - Vector3* p_FCs); +extern template +FCL_EXTERN_TEMPLATE_API +bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + double* distance, Vector3* p_FSc, + Vector3* p_FCs); //============================================================================== @@ -111,7 +113,7 @@ bool nearestPointInCylinder(const S& height, const S& radius, //============================================================================== template -FCL_EXPORT bool sphereCylinderIntersect( +bool sphereCylinderIntersect( const Sphere& sphere, const Transform3& X_FS, const Cylinder& cylinder, const Transform3& X_FC, std::vector>* contacts) { @@ -223,11 +225,11 @@ FCL_EXPORT bool sphereCylinderIntersect( //============================================================================== template -FCL_EXPORT bool sphereCylinderDistance(const Sphere& sphere, - const Transform3& X_FS, - const Cylinder& cylinder, - const Transform3& X_FC, S* distance, - Vector3* p_FSc, Vector3* p_FCs) { +bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, S* distance, + Vector3* p_FSc, Vector3* p_FCs) { // Find the sphere center S in the cylinder's frame. const Transform3 X_CS = X_FC.inverse() * X_FS; const Vector3 p_CS = X_CS.translation(); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h index 4209b03a0..7f4fe6dad 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.h @@ -102,11 +102,11 @@ namespace detail { @return True if the objects are colliding (including touching). @tparam S The scalar parameter (must be a valid Eigen scalar). */ template -FCL_EXPORT bool sphereCylinderIntersect(const Sphere& sphere, - const Transform3& X_FS, - const Cylinder& cylinder, - const Transform3& X_FC, - std::vector>* contacts); +bool sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); /** Evaluate the minimum separating distance between a sphere and cylinder. If separated, the nearest points on each shape will be returned in frame F. @@ -123,11 +123,11 @@ FCL_EXPORT bool sphereCylinderIntersect(const Sphere& sphere, @return True if the objects are separated. @tparam S The scalar parameter (must be a valid Eigen scalar). */ template -FCL_EXPORT bool sphereCylinderDistance(const Sphere& sphere, - const Transform3& X_FS, - const Cylinder& cylinder, - const Transform3& X_FC, S* distance, - Vector3* p_FSc, Vector3* p_FCs); +bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, S* distance, + Vector3* p_FSc, Vector3* p_FCs); //@} diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h index d12671cfb..9a5aa7f1b 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h @@ -48,21 +48,20 @@ namespace detail //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API bool sphereSphereIntersect(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template -FCL_EXPORT +FCL_EXTERN_TEMPLATE_API bool sphereSphereDistance(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); //============================================================================== template -FCL_EXPORT bool sphereSphereIntersect(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, std::vector>* contacts) @@ -87,7 +86,6 @@ bool sphereSphereIntersect(const Sphere& s1, const Transform3& tf1, //============================================================================== template -FCL_EXPORT bool sphereSphereDistance(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, S* dist, Vector3* p1, Vector3* p2) diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle-inl.h index 0078900df..2899110d5 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle-inl.h @@ -50,31 +50,37 @@ namespace detail //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double segmentSqrDistance(const Vector3& from, const Vector3& to,const Vector3& p, Vector3& nearest); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool projectInTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& normal, const Vector3& p); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool sphereTriangleIntersect(const Sphere& s, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, Vector3* contact_points, double* penetration_depth, Vector3* normal_); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, double* dist); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, double* dist, Vector3* p1, Vector3* p2); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h index 15a2a70ef..61da0d580 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.h @@ -49,33 +49,27 @@ namespace detail /** @brief the minimum distance from a point to a line */ template -FCL_EXPORT S segmentSqrDistance(const Vector3& from, const Vector3& to,const Vector3& p, Vector3& nearest); /// @brief Whether a point's projection is in a triangle template -FCL_EXPORT bool projectInTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& normal, const Vector3& p); template -FCL_EXPORT bool sphereTriangleIntersect(const Sphere& s, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, Vector3* contact_points, S* penetration_depth, Vector3* normal_); template -FCL_EXPORT bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, S* dist); template -FCL_EXPORT bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, S* dist, Vector3* p1, Vector3* p2); template -FCL_EXPORT bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, S* dist, Vector3* p1, Vector3* p2); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h index 33a1d4fd6..9beef7625 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h @@ -1,467 +1,467 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H -#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H - -#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -extern template -class FCL_EXPORT TriangleDistance; - -//============================================================================== -template -void TriangleDistance::segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, - Vector3& VEC, Vector3& X, Vector3& Y) -{ - Vector3 T; - S A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; - Vector3 TMP; - - T = Q - P; - A_dot_A = A.dot(A); - B_dot_B = B.dot(B); - A_dot_B = A.dot(B); - A_dot_T = A.dot(T); - B_dot_T = B.dot(T); - - // t parameterizes ray P,A - // u parameterizes ray Q,B - - S t, u; - - // compute t for the closest point on ray P,A to - // ray Q,B - - S denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; - - t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; - - // clamp result so t is on the segment P,A - - if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1; - - // find u for point on ray Q,B closest to point at t - - u = (t*A_dot_B - B_dot_T) / B_dot_B; - - // if u is on segment Q,B, t and u correspond to - // closest points, otherwise, clamp u, recompute and - // clamp t - - if((u <= 0) || std::isnan(u)) - { - Y = Q; - - t = A_dot_T / A_dot_A; - - if((t <= 0) || std::isnan(t)) - { - X = P; - VEC = Q - P; - } - else if(t >= 1) - { - X = P + A; - VEC = Q - X; - } - else - { - X = P + A * t; - TMP = T.cross(A); - VEC = A.cross(TMP); - } - } - else if (u >= 1) - { - Y = Q + B; - - t = (A_dot_B + A_dot_T) / A_dot_A; - - if((t <= 0) || std::isnan(t)) - { - X = P; - VEC = Y - P; - } - else if(t >= 1) - { - X = P + A; - VEC = Y - X; - } - else - { - X = P + A * t; - T = Y - P; - TMP = T.cross(A); - VEC= A.cross(TMP); - } - } - else - { - Y = Q + B * u; - - if((t <= 0) || std::isnan(t)) - { - X = P; - TMP = T.cross(B); - VEC = B.cross(TMP); - } - else if(t >= 1) - { - X = P + A; - T = Q - X; - TMP = T.cross(B); - VEC = B.cross(TMP); - } - else - { - X = P + A * t; - VEC = A.cross(B); - if(VEC.dot(T) < 0) - { - VEC = VEC * (-1); - } - } - } -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q) -{ - // Compute vectors along the 6 sides - - Vector3 Sv[3]; - Vector3 Tv[3]; - Vector3 VEC; - - Sv[0] = T1[1] - T1[0]; - Sv[1] = T1[2] - T1[1]; - Sv[2] = T1[0] - T1[2]; - - Tv[0] = T2[1] - T2[0]; - Tv[1] = T2[2] - T2[1]; - Tv[2] = T2[0] - T2[2]; - - // For each edge pair, the vector connecting the closest points - // of the edges defines a slab (parallel planes at head and tail - // enclose the slab). If we can show that the off-edge vertex of - // each triangle is outside of the slab, then the closest points - // of the edges are the closest points for the triangles. - // Even if these tests fail, it may be helpful to know the closest - // points found, and whether the triangles were shown disjoint - - Vector3 V; - Vector3 Z; - Vector3 minP = Vector3::Zero(); - Vector3 minQ = Vector3::Zero(); - S mindd; - int shown_disjoint = 0; - - mindd = (T1[0] - T2[0]).squaredNorm() + 1; // Set first minimum safely high - - for(int i = 0; i < 3; ++i) - { - for(int j = 0; j < 3; ++j) - { - // Find closest points on edges i & j, plus the - // vector (and distance squared) between these points - segPoints(T1[i], Sv[i], T2[j], Tv[j], VEC, P, Q); - - V = Q - P; - S dd = V.dot(V); - - // Verify this closest point pair only if the distance - // squared is less than the minimum found thus far. - - if(dd <= mindd) - { - minP = P; - minQ = Q; - mindd = dd; - - Z = T1[(i+2)%3] - P; - S a = Z.dot(VEC); - Z = T2[(j+2)%3] - Q; - S b = Z.dot(VEC); - - if((a <= 0) && (b >= 0)) return sqrt(dd); - - S p = V.dot(VEC); - - if(a < 0) a = 0; - if(b > 0) b = 0; - if((p - a + b) > 0) shown_disjoint = 1; - } - } - } - - // No edge pairs contained the closest points. - // either: - // 1. one of the closest points is a vertex, and the - // other point is interior to a face. - // 2. the triangles are overlapping. - // 3. an edge of one triangle is parallel to the other's face. If - // cases 1 and 2 are not true, then the closest points from the 9 - // edge pairs checks above can be taken as closest points for the - // triangles. - // 4. possibly, the triangles were degenerate. When the - // triangle points are nearly colinear or coincident, one - // of above tests might fail even though the edges tested - // contain the closest points. - - // First check for case 1 - - Vector3 Sn; - S Snl; - - Sn = Sv[0].cross(Sv[1]); // Compute normal to T1 triangle - Snl = Sn.dot(Sn); // Compute square of length of normal - - // If cross product is long enough, - - if(Snl > 1e-15) - { - // Get projection lengths of T2 points - - Vector3 Tp; - - V = T1[0] - T2[0]; - Tp[0] = V.dot(Sn); - - V = T1[0] - T2[1]; - Tp[1] = V.dot(Sn); - - V = T1[0] - T2[2]; - Tp[2] = V.dot(Sn); - - // If Sn is a separating direction, - // find point with smallest projection - - int point = -1; - if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) - { - if(Tp[0] < Tp[1]) point = 0; else point = 1; - if(Tp[2] < Tp[point]) point = 2; - } - else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) - { - if(Tp[0] > Tp[1]) point = 0; else point = 1; - if(Tp[2] > Tp[point]) point = 2; - } - - // If Sn is a separating direction, - - if(point >= 0) - { - shown_disjoint = 1; - - // Test whether the point found, when projected onto the - // other triangle, lies within the face. - - V = T2[point] - T1[0]; - Z = Sn.cross(Sv[0]); - if(V.dot(Z) > 0) - { - V = T2[point] - T1[1]; - Z = Sn.cross(Sv[1]); - if(V.dot(Z) > 0) - { - V = T2[point] - T1[2]; - Z = Sn.cross(Sv[2]); - if(V.dot(Z) > 0) - { - // T[point] passed the test - it's a closest point for - // the T2 triangle; the other point is on the face of T1 - P = T2[point] + Sn * (Tp[point] / Snl); - Q = T2[point]; - return (P - Q).norm(); - } - } - } - } - } - - Vector3 Tn; - S Tnl; - - Tn = Tv[0].cross(Tv[1]); - Tnl = Tn.dot(Tn); - - if(Tnl > 1e-15) - { - Vector3 Sp; - - V = T2[0] - T1[0]; - Sp[0] = V.dot(Tn); - - V = T2[0] - T1[1]; - Sp[1] = V.dot(Tn); - - V = T2[0] - T1[2]; - Sp[2] = V.dot(Tn); - - int point = -1; - if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) - { - if(Sp[0] < Sp[1]) point = 0; else point = 1; - if(Sp[2] < Sp[point]) point = 2; - } - else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) - { - if(Sp[0] > Sp[1]) point = 0; else point = 1; - if(Sp[2] > Sp[point]) point = 2; - } - - if(point >= 0) - { - shown_disjoint = 1; - - V = T1[point] - T2[0]; - Z = Tn.cross(Tv[0]); - if(V.dot(Z) > 0) - { - V = T1[point] - T2[1]; - Z = Tn.cross(Tv[1]); - if(V.dot(Z) > 0) - { - V = T1[point] - T2[2]; - Z = Tn.cross(Tv[2]); - if(V.dot(Z) > 0) - { - P = T1[point]; - Q = T1[point] + Tn * (Sp[point] / Tnl); - return (P - Q).norm(); - } - } - } - } - } - - // Case 1 can't be shown. - // If one of these tests showed the triangles disjoint, - // we assume case 3 or 4, otherwise we conclude case 2, - // that the triangles overlap. - - if(shown_disjoint) - { - P = minP; - Q = minQ; - return sqrt(mindd); - } - else return 0; -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - Vector3& P, Vector3& Q) -{ - Vector3 U[3]; - Vector3 T[3]; - U[0] = S1; U[1] = S2; U[2] = S3; - T[0] = T1; T[1] = T2; T[2] = T3; - - return triDistance(U, T, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q) -{ - Vector3 T_transformed[3]; - T_transformed[0] = R * T2[0] + Tl; - T_transformed[1] = R * T2[1] + Tl; - T_transformed[2] = R * T2[2] + Tl; - - return triDistance(T1, T_transformed, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Transform3& tf, - Vector3& P, Vector3& Q) -{ - Vector3 T_transformed[3]; - T_transformed[0] = tf * T2[0]; - T_transformed[1] = tf * T2[1]; - T_transformed[2] = tf * T2[2]; - - return triDistance(T1, T_transformed, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q) -{ - Vector3 T1_transformed = R * T1 + Tl; - Vector3 T2_transformed = R * T2 + Tl; - Vector3 T3_transformed = R * T3 + Tl; - return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - const Transform3& tf, - Vector3& P, Vector3& Q) -{ - Vector3 T1_transformed = tf * T1; - Vector3 T2_transformed = tf * T2; - Vector3 T3_transformed = tf * T3; - return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H +#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API TriangleDistance; + +//============================================================================== +template +void TriangleDistance::segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, + Vector3& VEC, Vector3& X, Vector3& Y) +{ + Vector3 T; + S A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; + Vector3 TMP; + + T = Q - P; + A_dot_A = A.dot(A); + B_dot_B = B.dot(B); + A_dot_B = A.dot(B); + A_dot_T = A.dot(T); + B_dot_T = B.dot(T); + + // t parameterizes ray P,A + // u parameterizes ray Q,B + + S t, u; + + // compute t for the closest point on ray P,A to + // ray Q,B + + S denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; + + t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; + + // clamp result so t is on the segment P,A + + if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1; + + // find u for point on ray Q,B closest to point at t + + u = (t*A_dot_B - B_dot_T) / B_dot_B; + + // if u is on segment Q,B, t and u correspond to + // closest points, otherwise, clamp u, recompute and + // clamp t + + if((u <= 0) || std::isnan(u)) + { + Y = Q; + + t = A_dot_T / A_dot_A; + + if((t <= 0) || std::isnan(t)) + { + X = P; + VEC = Q - P; + } + else if(t >= 1) + { + X = P + A; + VEC = Q - X; + } + else + { + X = P + A * t; + TMP = T.cross(A); + VEC = A.cross(TMP); + } + } + else if (u >= 1) + { + Y = Q + B; + + t = (A_dot_B + A_dot_T) / A_dot_A; + + if((t <= 0) || std::isnan(t)) + { + X = P; + VEC = Y - P; + } + else if(t >= 1) + { + X = P + A; + VEC = Y - X; + } + else + { + X = P + A * t; + T = Y - P; + TMP = T.cross(A); + VEC= A.cross(TMP); + } + } + else + { + Y = Q + B * u; + + if((t <= 0) || std::isnan(t)) + { + X = P; + TMP = T.cross(B); + VEC = B.cross(TMP); + } + else if(t >= 1) + { + X = P + A; + T = Q - X; + TMP = T.cross(B); + VEC = B.cross(TMP); + } + else + { + X = P + A * t; + VEC = A.cross(B); + if(VEC.dot(T) < 0) + { + VEC = VEC * (-1); + } + } + } +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q) +{ + // Compute vectors along the 6 sides + + Vector3 Sv[3]; + Vector3 Tv[3]; + Vector3 VEC; + + Sv[0] = T1[1] - T1[0]; + Sv[1] = T1[2] - T1[1]; + Sv[2] = T1[0] - T1[2]; + + Tv[0] = T2[1] - T2[0]; + Tv[1] = T2[2] - T2[1]; + Tv[2] = T2[0] - T2[2]; + + // For each edge pair, the vector connecting the closest points + // of the edges defines a slab (parallel planes at head and tail + // enclose the slab). If we can show that the off-edge vertex of + // each triangle is outside of the slab, then the closest points + // of the edges are the closest points for the triangles. + // Even if these tests fail, it may be helpful to know the closest + // points found, and whether the triangles were shown disjoint + + Vector3 V; + Vector3 Z; + Vector3 minP = Vector3::Zero(); + Vector3 minQ = Vector3::Zero(); + S mindd; + int shown_disjoint = 0; + + mindd = (T1[0] - T2[0]).squaredNorm() + 1; // Set first minimum safely high + + for(int i = 0; i < 3; ++i) + { + for(int j = 0; j < 3; ++j) + { + // Find closest points on edges i & j, plus the + // vector (and distance squared) between these points + segPoints(T1[i], Sv[i], T2[j], Tv[j], VEC, P, Q); + + V = Q - P; + S dd = V.dot(V); + + // Verify this closest point pair only if the distance + // squared is less than the minimum found thus far. + + if(dd <= mindd) + { + minP = P; + minQ = Q; + mindd = dd; + + Z = T1[(i+2)%3] - P; + S a = Z.dot(VEC); + Z = T2[(j+2)%3] - Q; + S b = Z.dot(VEC); + + if((a <= 0) && (b >= 0)) return sqrt(dd); + + S p = V.dot(VEC); + + if(a < 0) a = 0; + if(b > 0) b = 0; + if((p - a + b) > 0) shown_disjoint = 1; + } + } + } + + // No edge pairs contained the closest points. + // either: + // 1. one of the closest points is a vertex, and the + // other point is interior to a face. + // 2. the triangles are overlapping. + // 3. an edge of one triangle is parallel to the other's face. If + // cases 1 and 2 are not true, then the closest points from the 9 + // edge pairs checks above can be taken as closest points for the + // triangles. + // 4. possibly, the triangles were degenerate. When the + // triangle points are nearly colinear or coincident, one + // of above tests might fail even though the edges tested + // contain the closest points. + + // First check for case 1 + + Vector3 Sn; + S Snl; + + Sn = Sv[0].cross(Sv[1]); // Compute normal to T1 triangle + Snl = Sn.dot(Sn); // Compute square of length of normal + + // If cross product is long enough, + + if(Snl > 1e-15) + { + // Get projection lengths of T2 points + + Vector3 Tp; + + V = T1[0] - T2[0]; + Tp[0] = V.dot(Sn); + + V = T1[0] - T2[1]; + Tp[1] = V.dot(Sn); + + V = T1[0] - T2[2]; + Tp[2] = V.dot(Sn); + + // If Sn is a separating direction, + // find point with smallest projection + + int point = -1; + if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) + { + if(Tp[0] < Tp[1]) point = 0; else point = 1; + if(Tp[2] < Tp[point]) point = 2; + } + else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) + { + if(Tp[0] > Tp[1]) point = 0; else point = 1; + if(Tp[2] > Tp[point]) point = 2; + } + + // If Sn is a separating direction, + + if(point >= 0) + { + shown_disjoint = 1; + + // Test whether the point found, when projected onto the + // other triangle, lies within the face. + + V = T2[point] - T1[0]; + Z = Sn.cross(Sv[0]); + if(V.dot(Z) > 0) + { + V = T2[point] - T1[1]; + Z = Sn.cross(Sv[1]); + if(V.dot(Z) > 0) + { + V = T2[point] - T1[2]; + Z = Sn.cross(Sv[2]); + if(V.dot(Z) > 0) + { + // T[point] passed the test - it's a closest point for + // the T2 triangle; the other point is on the face of T1 + P = T2[point] + Sn * (Tp[point] / Snl); + Q = T2[point]; + return (P - Q).norm(); + } + } + } + } + } + + Vector3 Tn; + S Tnl; + + Tn = Tv[0].cross(Tv[1]); + Tnl = Tn.dot(Tn); + + if(Tnl > 1e-15) + { + Vector3 Sp; + + V = T2[0] - T1[0]; + Sp[0] = V.dot(Tn); + + V = T2[0] - T1[1]; + Sp[1] = V.dot(Tn); + + V = T2[0] - T1[2]; + Sp[2] = V.dot(Tn); + + int point = -1; + if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) + { + if(Sp[0] < Sp[1]) point = 0; else point = 1; + if(Sp[2] < Sp[point]) point = 2; + } + else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) + { + if(Sp[0] > Sp[1]) point = 0; else point = 1; + if(Sp[2] > Sp[point]) point = 2; + } + + if(point >= 0) + { + shown_disjoint = 1; + + V = T1[point] - T2[0]; + Z = Tn.cross(Tv[0]); + if(V.dot(Z) > 0) + { + V = T1[point] - T2[1]; + Z = Tn.cross(Tv[1]); + if(V.dot(Z) > 0) + { + V = T1[point] - T2[2]; + Z = Tn.cross(Tv[2]); + if(V.dot(Z) > 0) + { + P = T1[point]; + Q = T1[point] + Tn * (Sp[point] / Tnl); + return (P - Q).norm(); + } + } + } + } + } + + // Case 1 can't be shown. + // If one of these tests showed the triangles disjoint, + // we assume case 3 or 4, otherwise we conclude case 2, + // that the triangles overlap. + + if(shown_disjoint) + { + P = minP; + Q = minQ; + return sqrt(mindd); + } + else return 0; +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + Vector3& P, Vector3& Q) +{ + Vector3 U[3]; + Vector3 T[3]; + U[0] = S1; U[1] = S2; U[2] = S3; + T[0] = T1; T[1] = T2; T[2] = T3; + + return triDistance(U, T, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q) +{ + Vector3 T_transformed[3]; + T_transformed[0] = R * T2[0] + Tl; + T_transformed[1] = R * T2[1] + Tl; + T_transformed[2] = R * T2[2] + Tl; + + return triDistance(T1, T_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Transform3& tf, + Vector3& P, Vector3& Q) +{ + Vector3 T_transformed[3]; + T_transformed[0] = tf * T2[0]; + T_transformed[1] = tf * T2[1]; + T_transformed[2] = tf * T2[2]; + + return triDistance(T1, T_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q) +{ + Vector3 T1_transformed = R * T1 + Tl; + Vector3 T2_transformed = R * T2 + Tl; + Vector3 T3_transformed = R * T3 + Tl; + return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Transform3& tf, + Vector3& P, Vector3& Q) +{ + Vector3 T1_transformed = tf * T1; + Vector3 T2_transformed = tf * T2; + Vector3 T3_transformed = tf * T3; + return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h index cb4b2e693..b32408dc5 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h @@ -1,114 +1,114 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H -#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H - -#include "fcl/common/types.h" - -namespace fcl -{ - -namespace detail -{ - -/// @brief Triangle distance functions -template -class FCL_EXPORT TriangleDistance -{ -public: - - /// @brief Returns closest points between an segment pair. - /// The first segment is P + t * A - /// The second segment is Q + t * B - /// X, Y are the closest points on the two segments - /// VEC is the vector between X and Y - static void segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, - Vector3& VEC, Vector3& X, Vector3& Y); - - /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them - /// T1 and T2 are two triangles - /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not - /// coincident points on the intersection of the triangles, as might be expected. - static S triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q); - - static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - Vector3& P, Vector3& Q); - - /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them - /// T1 and T2 are two triangles - /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not - /// coincident points on the intersection of the triangles, as might be expected. - /// The returned P and Q are both in the coordinate of the first triangle's coordinate - static S triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q); - - static S triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Transform3& tf, - Vector3& P, Vector3& Q); - - static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q); - - static S triDistance( - const Vector3& S1, - const Vector3& S2, - const Vector3& S3, - const Vector3& T1, - const Vector3& T2, - const Vector3& T3, - const Transform3& tf, - Vector3& P, - Vector3& Q); - -}; - -using TriangleDistancef = TriangleDistance; -using TriangleDistanced = TriangleDistance; - -} // namespace detail -} // namespace fcl - -#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H +#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H + +#include "fcl/common/types.h" + +namespace fcl +{ + +namespace detail +{ + +/// @brief Triangle distance functions +template +class TriangleDistance +{ +public: + + /// @brief Returns closest points between an segment pair. + /// The first segment is P + t * A + /// The second segment is Q + t * B + /// X, Y are the closest points on the two segments + /// VEC is the vector between X and Y + static void segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, + Vector3& VEC, Vector3& X, Vector3& Y); + + /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them + /// T1 and T2 are two triangles + /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not + /// coincident points on the intersection of the triangles, as might be expected. + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q); + + static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + Vector3& P, Vector3& Q); + + /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them + /// T1 and T2 are two triangles + /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not + /// coincident points on the intersection of the triangles, as might be expected. + /// The returned P and Q are both in the coordinate of the first triangle's coordinate + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q); + + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Transform3& tf, + Vector3& P, Vector3& Q); + + static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q); + + static S triDistance( + const Vector3& S1, + const Vector3& S2, + const Vector3& S3, + const Vector3& T1, + const Vector3& T2, + const Vector3& T3, + const Transform3& tf, + Vector3& P, + Vector3& Q); + +}; + +using TriangleDistancef = TriangleDistance; +using TriangleDistanced = TriangleDistance; + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" + +#endif diff --git a/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h index 021a47823..d1473867e 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h @@ -49,7 +49,7 @@ namespace detail /// @brief Traversal node for collision between BVH models template -class FCL_EXPORT BVHCollisionTraversalNode +class BVHCollisionTraversalNode : public CollisionTraversalNodeBase { public: @@ -81,7 +81,7 @@ class FCL_EXPORT BVHCollisionTraversalNode /// @brief BV culling test in one BVTT node bool BVTesting(int b1, int b2) const; - + /// @brief The first BVH model const BVHModel* model1; diff --git a/include/fcl/narrowphase/detail/traversal/collision/bvh_shape_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/bvh_shape_collision_traversal_node.h index 9247d5622..551223d11 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/bvh_shape_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/bvh_shape_collision_traversal_node.h @@ -49,7 +49,7 @@ namespace detail /// @brief Traversal node for collision between BVH and shape template -class FCL_EXPORT BVHShapeCollisionTraversalNode +class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base-inl.h b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base-inl.h index 86743e2d7..75bddb675 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base-inl.h @@ -50,7 +50,7 @@ namespace detail //============================================================================== extern template -class FCL_EXPORT CollisionTraversalNodeBase; +class FCL_EXTERN_TEMPLATE_API CollisionTraversalNodeBase; //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h index 6f6b10ca6..959d532df 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h +++ b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h @@ -49,7 +49,7 @@ namespace detail /// @brief Node structure encoding the information required for collision traversal. template -class FCL_EXPORT CollisionTraversalNodeBase : public TraversalNodeBase +class CollisionTraversalNodeBase : public TraversalNodeBase { public: CollisionTraversalNodeBase(); @@ -74,7 +74,7 @@ class FCL_EXPORT CollisionTraversalNodeBase : public TraversalNodeBase /// @brief collision result kept during the traversal iteration CollisionResult* result; - /// @brief Whether stores statistics + /// @brief Whether stores statistics bool enable_statistics; }; diff --git a/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h b/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h index 4c9d6069f..7b7f02fda 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h @@ -1,1146 +1,1146 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H -#define FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H - -#include "fcl/narrowphase/detail/traversal/collision/intersect.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -extern template -class FCL_EXPORT Intersect; - -//============================================================================== -template -bool Intersect::isZero(S v) -{ - return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); -} - -//============================================================================== -/// @brief data: only used for EE, return the intersect point -template -bool Intersect::solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S& l, S& r, bool bVF, S coeffs[], Vector3* data) -{ - S v2[2]= {l*l,r*r}; - S v[2]= {l,r}; - S r_backup; - - unsigned char min3, min2, min1, max3, max2, max1; - - min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1; - min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1; - min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1; - - // bound the cubic - - S minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0]; - S major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0]; - - if(major<0) return false; - if(minor>0) return false; - - // starting here, the bounds have opposite values - S m = 0.5 * (r + l); - - // bound the derivative - S dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1]; - S dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1]; - - if((dminor > 0)||(dmajor < 0)) // we can use Newton - { - S m2 = m*m; - S fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0]; - S nl = m; - S nu = m; - if(fm>0) - { - nl-=(fm/dminor); - nu-=(fm/dmajor); - } - else - { - nu-=(fm/dminor); - nl-=(fm/dmajor); - } - - //intersect with [l,r] - - if(nl>r) return false; - if(nul) - { - if(nu -bool Intersect::insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p) -{ - Vector3 ab = b - a; - Vector3 ac = c - a; - Vector3 n = ab.cross(ac); - - Vector3 pa = a - p; - Vector3 pb = b - p; - Vector3 pc = c - p; - - if((pb.cross(pc)).dot(n) < -getEpsilon()) return false; - if((pc.cross(pa)).dot(n) < -getEpsilon()) return false; - if((pa.cross(pb)).dot(n) < -getEpsilon()) return false; - - return true; -} - -//============================================================================== -template -bool Intersect::insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p) -{ - return (p - a).dot(p - b) <= 0; -} - -//============================================================================== -/// @brief Calculate the line segment papb that is the shortest route between -/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where -/// pa = p1 + mua (p2 - p1) -/// pb = p3 + mub (p4 - p3) -/// Return FALSE if no solution exists. -template -bool Intersect::linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, - Vector3* pa, Vector3* pb, S* mua, S* mub) -{ - Vector3 p31 = p1 - p3; - Vector3 p34 = p4 - p3; - if(fabs(p34[0]) < getEpsilon() && fabs(p34[1]) < getEpsilon() && fabs(p34[2]) < getEpsilon()) - return false; - - Vector3 p12 = p2 - p1; - if(fabs(p12[0]) < getEpsilon() && fabs(p12[1]) < getEpsilon() && fabs(p12[2]) < getEpsilon()) - return false; - - S d3134 = p31.dot(p34); - S d3412 = p34.dot(p12); - S d3112 = p31.dot(p12); - S d3434 = p34.dot(p34); - S d1212 = p12.dot(p12); - - S denom = d1212 * d3434 - d3412 * d3412; - if(fabs(denom) < getEpsilon()) - return false; - S numer = d3134 * d3412 - d3112 * d3434; - - *mua = numer / denom; - if(*mua < 0 || *mua > 1) - return false; - - *mub = (d3134 + d3412 * (*mua)) / d3434; - if(*mub < 0 || *mub > 1) - return false; - - *pa = p1 + p12 * (*mua); - *pb = p3 + p34 * (*mub); - return true; -} - -//============================================================================== -template -bool Intersect::checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S t) -{ - return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t); -} - -//============================================================================== -template -bool Intersect::checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S t, Vector3* q_i) -{ - Vector3 a = a0 + va * t; - Vector3 b = b0 + vb * t; - Vector3 c = c0 + vc * t; - Vector3 d = d0 + vd * t; - Vector3 p1, p2; - S t_ab, t_cd; - if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd)) - { - if(q_i) *q_i = p1; - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - S t) -{ - return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t); -} - -//============================================================================== -template -bool Intersect::solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - bool bVF, - S* ret) -{ - S discriminant = b * b - 4 * a * c; - if(discriminant < 0) - return false; - - S sqrt_dis = sqrt(discriminant); - S r1 = (-b + sqrt_dis) / (2 * a); - bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false; - - S r2 = (-b - sqrt_dis) / (2 * a); - bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false; - - if(v1 && v2) - { - *ret = (r1 > r2) ? r2 : r1; - return true; - } - if(v1) - { - *ret = r1; - return true; - } - if(v2) - { - *ret = r2; - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp) -{ - if(isZero(a)) - { - S t = -c/b; - return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false; - } - - S discriminant = b*b-4*a*c; - if(discriminant < 0) - return false; - - S sqrt_dis = sqrt(discriminant); - - S r1 = (-b+sqrt_dis) / (2 * a); - bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false; - if(v1) return true; - - S r2 = (-b-sqrt_dis) / (2 * a); - bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false; - return v2; -} - -//============================================================================== -/// @brief Compute the cubic coefficients for VF case -/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. -template -void Intersect::computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S* a, S* b, S* c, S* d) -{ - Vector3 vavb = vb - va; - Vector3 vavc = vc - va; - Vector3 vavp = vp - va; - Vector3 a0b0 = b0 - a0; - Vector3 a0c0 = c0 - a0; - Vector3 a0p0 = p0 - a0; - - Vector3 vavb_cross_vavc = vavb.cross(vavc); - Vector3 vavb_cross_a0c0 = vavb.cross(a0c0); - Vector3 a0b0_cross_vavc = a0b0.cross(vavc); - Vector3 a0b0_cross_a0c0 = a0b0.cross(a0c0); - - *a = vavp.dot(vavb_cross_vavc); - *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); - *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); - *d = a0p0.dot(a0b0_cross_a0c0); -} - -//============================================================================== -template -void Intersect::computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S* a, S* b, S* c, S* d) -{ - Vector3 vavb = vb - va; - Vector3 vcvd = vd - vc; - Vector3 vavc = vc - va; - Vector3 c0d0 = d0 - c0; - Vector3 a0b0 = b0 - a0; - Vector3 a0c0 = c0 - a0; - Vector3 vavb_cross_vcvd = vavb.cross(vcvd); - Vector3 vavb_cross_c0d0 = vavb.cross(c0d0); - Vector3 a0b0_cross_vcvd = a0b0.cross(vcvd); - Vector3 a0b0_cross_c0d0 = a0b0.cross(c0d0); - - *a = vavc.dot(vavb_cross_vcvd); - *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); - *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); - *d = a0c0.dot(a0b0_cross_c0d0); -} - -//============================================================================== -template -void Intersect::computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - const Vector3& L, - S* a, S* b, S* c) -{ - Vector3 vbva = va - vb; - Vector3 vbvp = vp - vb; - Vector3 b0a0 = a0 - b0; - Vector3 b0p0 = p0 - b0; - - Vector3 L_cross_vbvp = L.cross(vbvp); - Vector3 L_cross_b0p0 = L.cross(b0p0); - - *a = L_cross_vbvp.dot(vbva); - *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); - *c = L_cross_b0p0.dot(b0a0); -} - -//============================================================================== -template -bool Intersect::intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - *collision_time = 2.0; - - Vector3 vp, va, vb, vc; - vp = p1 - p0; - va = a1 - a0; - vb = b1 - b0; - vc = c1 - c0; - - S a, b, c, d; - computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d); - - if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) - { - return false; - } - - - /// if(isZero(a)) - /// { - /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); - /// } - - S coeffs[4]; - coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; - - if(useNewton) - { - S l = 0; - S r = 1; - - if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs)) - { - *collision_time = 0.5 * (l + r); - } - } - else - { - S roots[3]; - int num = PolySolver::solveCubic(coeffs, roots); - for(int i = 0; i < num; ++i) - { - S r = roots[i]; - if(r < 0 || r > 1) continue; - if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r)) - { - *collision_time = r; - break; - } - } - } - - if(*collision_time > 1) - { - return false; - } - - *p_i = vp * (*collision_time) + p0; - return true; -} - -//============================================================================== -template -bool Intersect::intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - *collision_time = 2.0; - - Vector3 va, vb, vc, vd; - va = a1 - a0; - vb = b1 - b0; - vc = c1 - c0; - vd = d1 - d0; - - S a, b, c, d; - computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d); - - if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) - { - return false; - } - - /// if(isZero(a)) - /// { - /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); - /// } - - - S coeffs[4]; - coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; - - if(useNewton) - { - S l = 0; - S r = 1; - - if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i)) - { - *collision_time = (l + r) * 0.5; - } - } - else - { - S roots[3]; - int num = PolySolver::solveCubic(coeffs, roots); - for(int i = 0; i < num; ++i) - { - S r = roots[i]; - if(r < 0 || r > 1) continue; - - if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i)) - { - *collision_time = r; - break; - } - } - } - - if(*collision_time > 1) - { - return false; - } - - return true; -} - -//============================================================================== -template -bool Intersect::intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& p1, - const Vector3& L) -{ - Vector3 va, vb, vp; - va = a1 - a0; - vb = b1 - b0; - vp = p1 - p0; - - S a, b, c; - computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c); - - if(isZero(a) && isZero(b) && isZero(c)) - return true; - - return solveSquare(a, b, c, a0, b0, p0, va, vb, vp); - -} - -//============================================================================== -/// @brief Prefilter for intersection, works for both VF and EE -template -bool Intersect::intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1) -{ - Vector3 n0 = (b0 - a0).cross(c0 - a0); - Vector3 n1 = (b1 - a1).cross(c1 - a1); - Vector3 a0a1 = a1 - a0; - Vector3 b0b1 = b1 - b0; - Vector3 c0c1 = c1 - c0; - Vector3 delta = (b0b1 - a0a1).cross(c0c1 - a0a1); - Vector3 nx = (n0 + n1 - delta) * 0.5; - - Vector3 a0d0 = d0 - a0; - Vector3 a1d1 = d1 - a1; - - S A = n0.dot(a0d0); - S B = n1.dot(a1d1); - S C = nx.dot(a0d0); - S D = nx.dot(a1d1); - S E = n1.dot(a0d0); - S F = n0.dot(a1d1); - - if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0) - return false; - if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0) - return false; - - return true; -} - -//============================================================================== -template -bool Intersect::intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1)) - { - return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton); - } - else - return false; -} - -//============================================================================== -template -bool Intersect::intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1)) - { - return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton); - } - else - return false; -} - -//============================================================================== -template -bool Intersect::intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Matrix3& R, - const Vector3& T, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 Q1_ = R * Q1 + T; - Vector3 Q2_ = R * Q2 + T; - Vector3 Q3_ = R * Q3 + T; - - return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); -} - -//============================================================================== -template -bool Intersect::intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Transform3& tf, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 Q1_ = tf * Q1; - Vector3 Q2_ = tf * Q2; - Vector3 Q3_ = tf * Q3; - - return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); -} - -//============================================================================== -template -bool Intersect::intersect_Triangle_ODE_style( - const Vector3& P1, const Vector3& P2, const Vector3& P3, - const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 n1; - S t1; - bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1); - if(!b1) return false; - - Vector3 n2; - S t2; - bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); - if(!b2) return false; - - if(sameSideOfPlane(P1, P2, P3, n2, t2)) - return false; - - if(sameSideOfPlane(Q1, Q2, Q3, n1, t1)) - return false; - - Vector3 clipped_points1[getMaxTriangleClips()]; - unsigned int num_clipped_points1 = 0; - Vector3 clipped_points2[getMaxTriangleClips()]; - unsigned int num_clipped_points2 = 0; - - Vector3 deepest_points1[getMaxTriangleClips()]; - unsigned int num_deepest_points1 = 0; - Vector3 deepest_points2[getMaxTriangleClips()]; - unsigned int num_deepest_points2 = 0; - S penetration_depth1 = -1, penetration_depth2 = -1; - - clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2); - - if(num_clipped_points2 == 0) - return false; - - computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); - if(num_deepest_points2 == 0) - return false; - - clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1); - if(num_clipped_points1 == 0) - return false; - - computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); - if(num_deepest_points1 == 0) - return false; - - - /// Return contact information - if(contact_points && num_contact_points && penetration_depth && normal) - { - if(penetration_depth1 > penetration_depth2) - { - *num_contact_points = num_deepest_points2; - for(unsigned int i = 0; i < num_deepest_points2; ++i) - { - contact_points[i] = deepest_points2[i]; - } - - *normal = n1; - *penetration_depth = penetration_depth2; - } - else - { - *num_contact_points = num_deepest_points1; - for(unsigned int i = 0; i < num_deepest_points1; ++i) - { - contact_points[i] = deepest_points1[i]; - } - - *normal = -n2; - *penetration_depth = penetration_depth1; - } - } - - return true; -} - -//============================================================================== -template -bool Intersect::intersect_Triangle( - const Vector3& P1, const Vector3& P2, const Vector3& P3, - const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 p1 = P1 - P1; - Vector3 p2 = P2 - P1; - Vector3 p3 = P3 - P1; - Vector3 q1 = Q1 - P1; - Vector3 q2 = Q2 - P1; - Vector3 q3 = Q3 - P1; - - Vector3 e1 = p2 - p1; - Vector3 e2 = p3 - p2; - Vector3 n1 = e1.cross(e2); - if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 f1 = q2 - q1; - Vector3 f2 = q3 - q2; - Vector3 m1 = f1.cross(f2); - if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef11 = e1.cross(f1); - if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef12 = e1.cross(f2); - if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 f3 = q1 - q3; - Vector3 ef13 = e1.cross(f3); - if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef21 = e2.cross(f1); - if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef22 = e2.cross(f2); - if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef23 = e2.cross(f3); - if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 e3 = p1 - p3; - Vector3 ef31 = e3.cross(f1); - if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef32 = e3.cross(f2); - if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef33 = e3.cross(f3); - if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 g1 = e1.cross(n1); - if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 g2 = e2.cross(n1); - if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 g3 = e3.cross(n1); - if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 h1 = f1.cross(m1); - if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 h2 = f2.cross(m1); - if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 h3 = f3.cross(m1); - if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false; - - if(contact_points && num_contact_points && penetration_depth && normal) - { - Vector3 n1, n2; - S t1, t2; - buildTrianglePlane(P1, P2, P3, &n1, &t1); - buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); - - Vector3 deepest_points1[3]; - unsigned int num_deepest_points1 = 0; - Vector3 deepest_points2[3]; - unsigned int num_deepest_points2 = 0; - S penetration_depth1, penetration_depth2; - - Vector3 P[3] = {P1, P2, P3}; - Vector3 Q[3] = {Q1, Q2, Q3}; - - computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); - computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); - - - if(penetration_depth1 > penetration_depth2) - { - *num_contact_points = std::min(num_deepest_points2, (unsigned int)2); - for(unsigned int i = 0; i < *num_contact_points; ++i) - { - contact_points[i] = deepest_points2[i]; - } - - *normal = n1; - *penetration_depth = penetration_depth2; - } - else - { - *num_contact_points = std::min(num_deepest_points1, (unsigned int)2); - for(unsigned int i = 0; i < *num_contact_points; ++i) - { - contact_points[i] = deepest_points1[i]; - } - - *normal = -n2; - *penetration_depth = penetration_depth1; - } - } - - return true; -} - -//============================================================================== -template -void Intersect::computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points) -{ - *num_deepest_points = 0; - S max_depth = -std::numeric_limits::max(); - unsigned int num_deepest_points_ = 0; - unsigned int num_neg = 0; - unsigned int num_pos = 0; - unsigned int num_zero = 0; - - for(unsigned int i = 0; i < num_clipped_points; ++i) - { - S dist = -distanceToPlane(n, t, clipped_points[i]); - if(dist > getEpsilon()) num_pos++; - else if(dist < -getEpsilon()) num_neg++; - else num_zero++; - if(dist > max_depth) - { - max_depth = dist; - num_deepest_points_ = 1; - deepest_points[num_deepest_points_ - 1] = clipped_points[i]; - } - else if(dist + 1e-6 >= max_depth) - { - num_deepest_points_++; - deepest_points[num_deepest_points_ - 1] = clipped_points[i]; - } - } - - if(max_depth < -getEpsilon()) - num_deepest_points_ = 0; - - if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0))) - num_deepest_points_ = 0; - - *penetration_depth = max_depth; - *num_deepest_points = num_deepest_points_; -} - -//============================================================================== -template -void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, - const Vector3& t1, const Vector3& t2, const Vector3& t3, - const Vector3& tn, S to, - Vector3 clipped_points[], unsigned int* num_clipped_points, - bool clip_triangle) -{ - *num_clipped_points = 0; - Vector3 temp_clip[getMaxTriangleClips()]; - Vector3 temp_clip2[getMaxTriangleClips()]; - unsigned int num_temp_clip = 0; - unsigned int num_temp_clip2 = 0; - Vector3 v[3] = {v1, v2, v3}; - - Vector3 plane_n; - S plane_dist; - - if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist)) - { - clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip); - if(num_temp_clip > 0) - { - if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist)) - { - clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2); - if(num_temp_clip2 > 0) - { - if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist)) - { - if(clip_triangle) - { - num_temp_clip = 0; - clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip); - if(num_temp_clip > 0) - { - clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points); - } - } - else - { - clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points); - } - } - } - } - } - } -} - -//============================================================================== -template -void Intersect::clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points) -{ - *num_clipped_points = 0; - - unsigned int num_clipped_points_ = 0; - unsigned int vi; - unsigned int prev_classify = 2; - unsigned int classify; - for(unsigned int i = 0; i <= num_polygon_points; ++i) - { - vi = (i % num_polygon_points); - S d = distanceToPlane(n, t, polygon_points[i]); - classify = ((d > getEpsilon()) ? 1 : 0); - if(classify == 0) - { - if(prev_classify == 1) - { - if(num_clipped_points_ < getMaxTriangleClips()) - { - Vector3 tmp; - clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); - if(num_clipped_points_ > 0) - { - if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - else - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - } - - if(num_clipped_points_ < getMaxTriangleClips() && i < num_polygon_points) - { - clipped_points[num_clipped_points_] = polygon_points[vi]; - num_clipped_points_++; - } - } - else - { - if(prev_classify == 0) - { - if(num_clipped_points_ < getMaxTriangleClips()) - { - Vector3 tmp; - clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); - if(num_clipped_points_ > 0) - { - if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - else - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - } - } - - prev_classify = classify; - } - - if(num_clipped_points_ > 2) - { - if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < getEpsilon()) - { - num_clipped_points_--; - } - } - - *num_clipped_points = num_clipped_points_; -} - -//============================================================================== -template -void Intersect::clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point) -{ - S dist1 = distanceToPlane(n, t, v1); - Vector3 tmp = v2 - v1; - S dist2 = tmp.dot(n); - *clipped_point = tmp * (-dist1 / dist2) + v1; -} - -//============================================================================== -template -S Intersect::distanceToPlane(const Vector3& n, S t, const Vector3& v) -{ - return n.dot(v) - t; -} - -//============================================================================== -template -bool Intersect::buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t) -{ - Vector3 n_ = (v2 - v1).cross(v3 - v1); - bool can_normalize = false; - normalize(n_, &can_normalize); - if(can_normalize) - { - *n = n_; - *t = n_.dot(v1); - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t) -{ - Vector3 n_ = (v2 - v1).cross(tn); - bool can_normalize = false; - normalize(n_, &can_normalize); - if(can_normalize) - { - *n = n_; - *t = n_.dot(v1); - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t) -{ - S dist1 = distanceToPlane(n, t, v1); - S dist2 = dist1 * distanceToPlane(n, t, v2); - S dist3 = dist1 * distanceToPlane(n, t, v3); - if((dist2 > 0) && (dist3 > 0)) - return true; - return false; -} - -//============================================================================== -template -int Intersect::project6(const Vector3& ax, - const Vector3& p1, const Vector3& p2, const Vector3& p3, - const Vector3& q1, const Vector3& q2, const Vector3& q3) -{ - S P1 = ax.dot(p1); - S P2 = ax.dot(p2); - S P3 = ax.dot(p3); - S Q1 = ax.dot(q1); - S Q2 = ax.dot(q2); - S Q3 = ax.dot(q3); - - S mn1 = std::min(P1, std::min(P2, P3)); - S mx2 = std::max(Q1, std::max(Q2, Q3)); - if(mn1 > mx2) return 0; - - S mx1 = std::max(P1, std::max(P2, P3)); - S mn2 = std::min(Q1, std::min(Q2, Q3)); - - if(mn2 > mx1) return 0; - return 1; -} - -//============================================================================== -template -S Intersect::gaussianCDF(S x) -{ - return 0.5 * std::erfc(-x / sqrt(2.0)); -} - -//============================================================================== -template -constexpr S Intersect::getEpsilon() -{ - return 1e-5; -} - -//============================================================================== -template -constexpr S Intersect::getNearZeroThreshold() -{ - return 1e-7; -} - -//============================================================================== -template -constexpr S Intersect::getCcdResolution() -{ - return 1e-7; -} - -//============================================================================== -template -constexpr unsigned int Intersect::getMaxTriangleClips() -{ - return 8; -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H +#define FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H + +#include "fcl/narrowphase/detail/traversal/collision/intersect.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API Intersect; + +//============================================================================== +template +bool Intersect::isZero(S v) +{ + return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); +} + +//============================================================================== +/// @brief data: only used for EE, return the intersect point +template +bool Intersect::solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S& l, S& r, bool bVF, S coeffs[], Vector3* data) +{ + S v2[2]= {l*l,r*r}; + S v[2]= {l,r}; + S r_backup; + + unsigned char min3, min2, min1, max3, max2, max1; + + min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1; + min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1; + min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1; + + // bound the cubic + + S minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0]; + S major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0]; + + if(major<0) return false; + if(minor>0) return false; + + // starting here, the bounds have opposite values + S m = 0.5 * (r + l); + + // bound the derivative + S dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1]; + S dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1]; + + if((dminor > 0)||(dmajor < 0)) // we can use Newton + { + S m2 = m*m; + S fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0]; + S nl = m; + S nu = m; + if(fm>0) + { + nl-=(fm/dminor); + nu-=(fm/dmajor); + } + else + { + nu-=(fm/dminor); + nl-=(fm/dmajor); + } + + //intersect with [l,r] + + if(nl>r) return false; + if(nul) + { + if(nu +bool Intersect::insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p) +{ + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 n = ab.cross(ac); + + Vector3 pa = a - p; + Vector3 pb = b - p; + Vector3 pc = c - p; + + if((pb.cross(pc)).dot(n) < -getEpsilon()) return false; + if((pc.cross(pa)).dot(n) < -getEpsilon()) return false; + if((pa.cross(pb)).dot(n) < -getEpsilon()) return false; + + return true; +} + +//============================================================================== +template +bool Intersect::insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p) +{ + return (p - a).dot(p - b) <= 0; +} + +//============================================================================== +/// @brief Calculate the line segment papb that is the shortest route between +/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where +/// pa = p1 + mua (p2 - p1) +/// pb = p3 + mub (p4 - p3) +/// Return FALSE if no solution exists. +template +bool Intersect::linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, + Vector3* pa, Vector3* pb, S* mua, S* mub) +{ + Vector3 p31 = p1 - p3; + Vector3 p34 = p4 - p3; + if(fabs(p34[0]) < getEpsilon() && fabs(p34[1]) < getEpsilon() && fabs(p34[2]) < getEpsilon()) + return false; + + Vector3 p12 = p2 - p1; + if(fabs(p12[0]) < getEpsilon() && fabs(p12[1]) < getEpsilon() && fabs(p12[2]) < getEpsilon()) + return false; + + S d3134 = p31.dot(p34); + S d3412 = p34.dot(p12); + S d3112 = p31.dot(p12); + S d3434 = p34.dot(p34); + S d1212 = p12.dot(p12); + + S denom = d1212 * d3434 - d3412 * d3412; + if(fabs(denom) < getEpsilon()) + return false; + S numer = d3134 * d3412 - d3112 * d3434; + + *mua = numer / denom; + if(*mua < 0 || *mua > 1) + return false; + + *mub = (d3134 + d3412 * (*mua)) / d3434; + if(*mub < 0 || *mub > 1) + return false; + + *pa = p1 + p12 * (*mua); + *pb = p3 + p34 * (*mub); + return true; +} + +//============================================================================== +template +bool Intersect::checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S t) +{ + return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t); +} + +//============================================================================== +template +bool Intersect::checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S t, Vector3* q_i) +{ + Vector3 a = a0 + va * t; + Vector3 b = b0 + vb * t; + Vector3 c = c0 + vc * t; + Vector3 d = d0 + vd * t; + Vector3 p1, p2; + S t_ab, t_cd; + if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd)) + { + if(q_i) *q_i = p1; + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + S t) +{ + return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t); +} + +//============================================================================== +template +bool Intersect::solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + bool bVF, + S* ret) +{ + S discriminant = b * b - 4 * a * c; + if(discriminant < 0) + return false; + + S sqrt_dis = sqrt(discriminant); + S r1 = (-b + sqrt_dis) / (2 * a); + bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false; + + S r2 = (-b - sqrt_dis) / (2 * a); + bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false; + + if(v1 && v2) + { + *ret = (r1 > r2) ? r2 : r1; + return true; + } + if(v1) + { + *ret = r1; + return true; + } + if(v2) + { + *ret = r2; + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp) +{ + if(isZero(a)) + { + S t = -c/b; + return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false; + } + + S discriminant = b*b-4*a*c; + if(discriminant < 0) + return false; + + S sqrt_dis = sqrt(discriminant); + + S r1 = (-b+sqrt_dis) / (2 * a); + bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false; + if(v1) return true; + + S r2 = (-b-sqrt_dis) / (2 * a); + bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false; + return v2; +} + +//============================================================================== +/// @brief Compute the cubic coefficients for VF case +/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. +template +void Intersect::computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S* a, S* b, S* c, S* d) +{ + Vector3 vavb = vb - va; + Vector3 vavc = vc - va; + Vector3 vavp = vp - va; + Vector3 a0b0 = b0 - a0; + Vector3 a0c0 = c0 - a0; + Vector3 a0p0 = p0 - a0; + + Vector3 vavb_cross_vavc = vavb.cross(vavc); + Vector3 vavb_cross_a0c0 = vavb.cross(a0c0); + Vector3 a0b0_cross_vavc = a0b0.cross(vavc); + Vector3 a0b0_cross_a0c0 = a0b0.cross(a0c0); + + *a = vavp.dot(vavb_cross_vavc); + *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); + *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); + *d = a0p0.dot(a0b0_cross_a0c0); +} + +//============================================================================== +template +void Intersect::computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S* a, S* b, S* c, S* d) +{ + Vector3 vavb = vb - va; + Vector3 vcvd = vd - vc; + Vector3 vavc = vc - va; + Vector3 c0d0 = d0 - c0; + Vector3 a0b0 = b0 - a0; + Vector3 a0c0 = c0 - a0; + Vector3 vavb_cross_vcvd = vavb.cross(vcvd); + Vector3 vavb_cross_c0d0 = vavb.cross(c0d0); + Vector3 a0b0_cross_vcvd = a0b0.cross(vcvd); + Vector3 a0b0_cross_c0d0 = a0b0.cross(c0d0); + + *a = vavc.dot(vavb_cross_vcvd); + *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); + *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); + *d = a0c0.dot(a0b0_cross_c0d0); +} + +//============================================================================== +template +void Intersect::computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + const Vector3& L, + S* a, S* b, S* c) +{ + Vector3 vbva = va - vb; + Vector3 vbvp = vp - vb; + Vector3 b0a0 = a0 - b0; + Vector3 b0p0 = p0 - b0; + + Vector3 L_cross_vbvp = L.cross(vbvp); + Vector3 L_cross_b0p0 = L.cross(b0p0); + + *a = L_cross_vbvp.dot(vbva); + *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); + *c = L_cross_b0p0.dot(b0a0); +} + +//============================================================================== +template +bool Intersect::intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + *collision_time = 2.0; + + Vector3 vp, va, vb, vc; + vp = p1 - p0; + va = a1 - a0; + vb = b1 - b0; + vc = c1 - c0; + + S a, b, c, d; + computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d); + + if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) + { + return false; + } + + + /// if(isZero(a)) + /// { + /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); + /// } + + S coeffs[4]; + coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; + + if(useNewton) + { + S l = 0; + S r = 1; + + if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs)) + { + *collision_time = 0.5 * (l + r); + } + } + else + { + S roots[3]; + int num = PolySolver::solveCubic(coeffs, roots); + for(int i = 0; i < num; ++i) + { + S r = roots[i]; + if(r < 0 || r > 1) continue; + if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r)) + { + *collision_time = r; + break; + } + } + } + + if(*collision_time > 1) + { + return false; + } + + *p_i = vp * (*collision_time) + p0; + return true; +} + +//============================================================================== +template +bool Intersect::intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + *collision_time = 2.0; + + Vector3 va, vb, vc, vd; + va = a1 - a0; + vb = b1 - b0; + vc = c1 - c0; + vd = d1 - d0; + + S a, b, c, d; + computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d); + + if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) + { + return false; + } + + /// if(isZero(a)) + /// { + /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); + /// } + + + S coeffs[4]; + coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; + + if(useNewton) + { + S l = 0; + S r = 1; + + if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i)) + { + *collision_time = (l + r) * 0.5; + } + } + else + { + S roots[3]; + int num = PolySolver::solveCubic(coeffs, roots); + for(int i = 0; i < num; ++i) + { + S r = roots[i]; + if(r < 0 || r > 1) continue; + + if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i)) + { + *collision_time = r; + break; + } + } + } + + if(*collision_time > 1) + { + return false; + } + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& p1, + const Vector3& L) +{ + Vector3 va, vb, vp; + va = a1 - a0; + vb = b1 - b0; + vp = p1 - p0; + + S a, b, c; + computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c); + + if(isZero(a) && isZero(b) && isZero(c)) + return true; + + return solveSquare(a, b, c, a0, b0, p0, va, vb, vp); + +} + +//============================================================================== +/// @brief Prefilter for intersection, works for both VF and EE +template +bool Intersect::intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1) +{ + Vector3 n0 = (b0 - a0).cross(c0 - a0); + Vector3 n1 = (b1 - a1).cross(c1 - a1); + Vector3 a0a1 = a1 - a0; + Vector3 b0b1 = b1 - b0; + Vector3 c0c1 = c1 - c0; + Vector3 delta = (b0b1 - a0a1).cross(c0c1 - a0a1); + Vector3 nx = (n0 + n1 - delta) * 0.5; + + Vector3 a0d0 = d0 - a0; + Vector3 a1d1 = d1 - a1; + + S A = n0.dot(a0d0); + S B = n1.dot(a1d1); + S C = nx.dot(a0d0); + S D = nx.dot(a1d1); + S E = n1.dot(a0d0); + S F = n0.dot(a1d1); + + if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0) + return false; + if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0) + return false; + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1)) + { + return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton); + } + else + return false; +} + +//============================================================================== +template +bool Intersect::intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1)) + { + return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton); + } + else + return false; +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Matrix3& R, + const Vector3& T, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 Q1_ = R * Q1 + T; + Vector3 Q2_ = R * Q2 + T; + Vector3 Q3_ = R * Q3 + T; + + return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Transform3& tf, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 Q1_ = tf * Q1; + Vector3 Q2_ = tf * Q2; + Vector3 Q3_ = tf * Q3; + + return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); +} + +//============================================================================== +template +bool Intersect::intersect_Triangle_ODE_style( + const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 n1; + S t1; + bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1); + if(!b1) return false; + + Vector3 n2; + S t2; + bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); + if(!b2) return false; + + if(sameSideOfPlane(P1, P2, P3, n2, t2)) + return false; + + if(sameSideOfPlane(Q1, Q2, Q3, n1, t1)) + return false; + + Vector3 clipped_points1[getMaxTriangleClips()]; + unsigned int num_clipped_points1 = 0; + Vector3 clipped_points2[getMaxTriangleClips()]; + unsigned int num_clipped_points2 = 0; + + Vector3 deepest_points1[getMaxTriangleClips()]; + unsigned int num_deepest_points1 = 0; + Vector3 deepest_points2[getMaxTriangleClips()]; + unsigned int num_deepest_points2 = 0; + S penetration_depth1 = -1, penetration_depth2 = -1; + + clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2); + + if(num_clipped_points2 == 0) + return false; + + computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); + if(num_deepest_points2 == 0) + return false; + + clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1); + if(num_clipped_points1 == 0) + return false; + + computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); + if(num_deepest_points1 == 0) + return false; + + + /// Return contact information + if(contact_points && num_contact_points && penetration_depth && normal) + { + if(penetration_depth1 > penetration_depth2) + { + *num_contact_points = num_deepest_points2; + for(unsigned int i = 0; i < num_deepest_points2; ++i) + { + contact_points[i] = deepest_points2[i]; + } + + *normal = n1; + *penetration_depth = penetration_depth2; + } + else + { + *num_contact_points = num_deepest_points1; + for(unsigned int i = 0; i < num_deepest_points1; ++i) + { + contact_points[i] = deepest_points1[i]; + } + + *normal = -n2; + *penetration_depth = penetration_depth1; + } + } + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 p1 = P1 - P1; + Vector3 p2 = P2 - P1; + Vector3 p3 = P3 - P1; + Vector3 q1 = Q1 - P1; + Vector3 q2 = Q2 - P1; + Vector3 q3 = Q3 - P1; + + Vector3 e1 = p2 - p1; + Vector3 e2 = p3 - p2; + Vector3 n1 = e1.cross(e2); + if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 f1 = q2 - q1; + Vector3 f2 = q3 - q2; + Vector3 m1 = f1.cross(f2); + if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef11 = e1.cross(f1); + if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef12 = e1.cross(f2); + if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 f3 = q1 - q3; + Vector3 ef13 = e1.cross(f3); + if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef21 = e2.cross(f1); + if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef22 = e2.cross(f2); + if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef23 = e2.cross(f3); + if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 e3 = p1 - p3; + Vector3 ef31 = e3.cross(f1); + if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef32 = e3.cross(f2); + if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef33 = e3.cross(f3); + if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g1 = e1.cross(n1); + if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g2 = e2.cross(n1); + if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g3 = e3.cross(n1); + if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h1 = f1.cross(m1); + if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h2 = f2.cross(m1); + if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h3 = f3.cross(m1); + if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false; + + if(contact_points && num_contact_points && penetration_depth && normal) + { + Vector3 n1, n2; + S t1, t2; + buildTrianglePlane(P1, P2, P3, &n1, &t1); + buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); + + Vector3 deepest_points1[3]; + unsigned int num_deepest_points1 = 0; + Vector3 deepest_points2[3]; + unsigned int num_deepest_points2 = 0; + S penetration_depth1, penetration_depth2; + + Vector3 P[3] = {P1, P2, P3}; + Vector3 Q[3] = {Q1, Q2, Q3}; + + computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); + computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); + + + if(penetration_depth1 > penetration_depth2) + { + *num_contact_points = std::min(num_deepest_points2, (unsigned int)2); + for(unsigned int i = 0; i < *num_contact_points; ++i) + { + contact_points[i] = deepest_points2[i]; + } + + *normal = n1; + *penetration_depth = penetration_depth2; + } + else + { + *num_contact_points = std::min(num_deepest_points1, (unsigned int)2); + for(unsigned int i = 0; i < *num_contact_points; ++i) + { + contact_points[i] = deepest_points1[i]; + } + + *normal = -n2; + *penetration_depth = penetration_depth1; + } + } + + return true; +} + +//============================================================================== +template +void Intersect::computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points) +{ + *num_deepest_points = 0; + S max_depth = -std::numeric_limits::max(); + unsigned int num_deepest_points_ = 0; + unsigned int num_neg = 0; + unsigned int num_pos = 0; + unsigned int num_zero = 0; + + for(unsigned int i = 0; i < num_clipped_points; ++i) + { + S dist = -distanceToPlane(n, t, clipped_points[i]); + if(dist > getEpsilon()) num_pos++; + else if(dist < -getEpsilon()) num_neg++; + else num_zero++; + if(dist > max_depth) + { + max_depth = dist; + num_deepest_points_ = 1; + deepest_points[num_deepest_points_ - 1] = clipped_points[i]; + } + else if(dist + 1e-6 >= max_depth) + { + num_deepest_points_++; + deepest_points[num_deepest_points_ - 1] = clipped_points[i]; + } + } + + if(max_depth < -getEpsilon()) + num_deepest_points_ = 0; + + if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0))) + num_deepest_points_ = 0; + + *penetration_depth = max_depth; + *num_deepest_points = num_deepest_points_; +} + +//============================================================================== +template +void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, + const Vector3& t1, const Vector3& t2, const Vector3& t3, + const Vector3& tn, S to, + Vector3 clipped_points[], unsigned int* num_clipped_points, + bool clip_triangle) +{ + *num_clipped_points = 0; + Vector3 temp_clip[getMaxTriangleClips()]; + Vector3 temp_clip2[getMaxTriangleClips()]; + unsigned int num_temp_clip = 0; + unsigned int num_temp_clip2 = 0; + Vector3 v[3] = {v1, v2, v3}; + + Vector3 plane_n; + S plane_dist; + + if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist)) + { + clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip); + if(num_temp_clip > 0) + { + if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist)) + { + clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2); + if(num_temp_clip2 > 0) + { + if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist)) + { + if(clip_triangle) + { + num_temp_clip = 0; + clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip); + if(num_temp_clip > 0) + { + clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points); + } + } + else + { + clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points); + } + } + } + } + } + } +} + +//============================================================================== +template +void Intersect::clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points) +{ + *num_clipped_points = 0; + + unsigned int num_clipped_points_ = 0; + unsigned int vi; + unsigned int prev_classify = 2; + unsigned int classify; + for(unsigned int i = 0; i <= num_polygon_points; ++i) + { + vi = (i % num_polygon_points); + S d = distanceToPlane(n, t, polygon_points[i]); + classify = ((d > getEpsilon()) ? 1 : 0); + if(classify == 0) + { + if(prev_classify == 1) + { + if(num_clipped_points_ < getMaxTriangleClips()) + { + Vector3 tmp; + clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); + if(num_clipped_points_ > 0) + { + if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + else + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + } + + if(num_clipped_points_ < getMaxTriangleClips() && i < num_polygon_points) + { + clipped_points[num_clipped_points_] = polygon_points[vi]; + num_clipped_points_++; + } + } + else + { + if(prev_classify == 0) + { + if(num_clipped_points_ < getMaxTriangleClips()) + { + Vector3 tmp; + clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); + if(num_clipped_points_ > 0) + { + if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + else + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + } + } + + prev_classify = classify; + } + + if(num_clipped_points_ > 2) + { + if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < getEpsilon()) + { + num_clipped_points_--; + } + } + + *num_clipped_points = num_clipped_points_; +} + +//============================================================================== +template +void Intersect::clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point) +{ + S dist1 = distanceToPlane(n, t, v1); + Vector3 tmp = v2 - v1; + S dist2 = tmp.dot(n); + *clipped_point = tmp * (-dist1 / dist2) + v1; +} + +//============================================================================== +template +S Intersect::distanceToPlane(const Vector3& n, S t, const Vector3& v) +{ + return n.dot(v) - t; +} + +//============================================================================== +template +bool Intersect::buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t) +{ + Vector3 n_ = (v2 - v1).cross(v3 - v1); + bool can_normalize = false; + normalize(n_, &can_normalize); + if(can_normalize) + { + *n = n_; + *t = n_.dot(v1); + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t) +{ + Vector3 n_ = (v2 - v1).cross(tn); + bool can_normalize = false; + normalize(n_, &can_normalize); + if(can_normalize) + { + *n = n_; + *t = n_.dot(v1); + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t) +{ + S dist1 = distanceToPlane(n, t, v1); + S dist2 = dist1 * distanceToPlane(n, t, v2); + S dist3 = dist1 * distanceToPlane(n, t, v3); + if((dist2 > 0) && (dist3 > 0)) + return true; + return false; +} + +//============================================================================== +template +int Intersect::project6(const Vector3& ax, + const Vector3& p1, const Vector3& p2, const Vector3& p3, + const Vector3& q1, const Vector3& q2, const Vector3& q3) +{ + S P1 = ax.dot(p1); + S P2 = ax.dot(p2); + S P3 = ax.dot(p3); + S Q1 = ax.dot(q1); + S Q2 = ax.dot(q2); + S Q3 = ax.dot(q3); + + S mn1 = std::min(P1, std::min(P2, P3)); + S mx2 = std::max(Q1, std::max(Q2, Q3)); + if(mn1 > mx2) return 0; + + S mx1 = std::max(P1, std::max(P2, P3)); + S mn2 = std::min(Q1, std::min(Q2, Q3)); + + if(mn2 > mx1) return 0; + return 1; +} + +//============================================================================== +template +S Intersect::gaussianCDF(S x) +{ + return 0.5 * std::erfc(-x / sqrt(2.0)); +} + +//============================================================================== +template +constexpr S Intersect::getEpsilon() +{ + return 1e-5; +} + +//============================================================================== +template +constexpr S Intersect::getNearZeroThreshold() +{ + return 1e-7; +} + +//============================================================================== +template +constexpr S Intersect::getCcdResolution() +{ + return 1e-7; +} + +//============================================================================== +template +constexpr unsigned int Intersect::getMaxTriangleClips() +{ + return 8; +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/traversal/collision/intersect.h b/include/fcl/narrowphase/detail/traversal/collision/intersect.h index d653b8793..df33a8cdc 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/intersect.h +++ b/include/fcl/narrowphase/detail/traversal/collision/intersect.h @@ -1,265 +1,265 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_INTERSECT_H -#define FCL_NARROWPHASE_DETAIL_INTERSECT_H - -#include -#include "fcl/common/types.h" -#include "fcl/math/geometry.h" -#include "fcl/math/detail/polysolver.h" - -namespace fcl -{ - -namespace detail -{ - -/// @brief CCD intersect kernel among primitives -template -class FCL_EXPORT Intersect -{ - -public: - - /// @brief CCD intersect between one vertex and one face - /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 - /// p0 and p1 are points for vertex in time t0 and t1 - /// p_i returns the coordinate of the collision point - static bool intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between two edges - /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 - /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 - /// p_i returns the coordinate of the collision point - static bool intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between one vertex and one face, using additional filter - static bool intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between two edges, using additional filter - static bool intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between one vertex and and one edge - static bool intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& p1, - const Vector3& L); - - /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] - static bool intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - - /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] - static bool intersect_Triangle_ODE_style( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - - static bool intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Matrix3& R, - const Vector3& T, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - - static bool intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Transform3& tf, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - -private: - - /// @brief Project function used in intersect_Triangle() - static int project6(const Vector3& ax, - const Vector3& p1, const Vector3& p2, const Vector3& p3, - const Vector3& q1, const Vector3& q2, const Vector3& q3); - - /// @brief Check whether one value is zero - static bool isZero(S v); - - /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction - static bool solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S& l, S& r, bool bVF, S coeffs[], Vector3* data = nullptr); - - /// @brief Check whether one point p is within triangle [a, b, c] - static bool insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p); - - /// @brief Check whether one point p is within a line segment [a, b] - static bool insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p); - - /// @brief Calculate the line segment papb that is the shortest route between - /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where - /// pa = p1 + mua (p2 - p1) - /// pb = p3 + mub (p4 - p3) - /// return FALSE if no solution exists. - static bool linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, - Vector3* pa, Vector3* pb, S* mua, S* mub); - - /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t - static bool checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S t); - - /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time - static bool checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S t, Vector3* q_i = nullptr); - - /// @brief Check whether a root for VE intersection is valid - static bool checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - S t); - - /// @brief Solve a square function for EE intersection (with interval restriction) - static bool solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - bool bVF, - S* ret); - - /// @brief Solve a square function for VE intersection (with interval restriction) - static bool solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp); - - /// @brief Compute the cubic coefficients for VF intersection - /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. - - static void computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S* a, S* b, S* c, S* d); - - /// @brief Compute the cubic coefficients for EE intersection - static void computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S* a, S* b, S* c, S* d); - - /// @brief Compute the cubic coefficients for VE intersection - static void computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - const Vector3& L, - S* a, S* b, S* c); - - /// @brief filter for intersection, works for both VF and EE - static bool intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1); - - /// @brief distance of point v to a plane n * x - t = 0 - static S distanceToPlane(const Vector3& n, S t, const Vector3& v); - - /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 - static bool sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t); - - /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to - static void clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, - const Vector3& t1, const Vector3& t2, const Vector3& t3, - const Vector3& tn, S to, - Vector3 clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false); - - /// @brief build a plane passed through triangle v1 v2 v3 - static bool buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t); - - /// @brief build a plane pass through edge v1 and v2, normal is tn - static bool buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t); - - /// @brief compute the points which has deepest penetration depth - static void computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points); - - /// @brief clip polygon by plane - static void clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points); - - /// @brief clip a line segment by plane - static void clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point); - - /// @brief compute the cdf(x) - static S gaussianCDF(S x); - - static constexpr S getEpsilon(); - static constexpr S getNearZeroThreshold(); - static constexpr S getCcdResolution(); - static constexpr unsigned int getMaxTriangleClips(); -}; - -using Intersectf = Intersect; -using Intersectd = Intersect; - -} // namespace detail -} // namespace fcl - -#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_INTERSECT_H +#define FCL_NARROWPHASE_DETAIL_INTERSECT_H + +#include +#include "fcl/common/types.h" +#include "fcl/math/geometry.h" +#include "fcl/math/detail/polysolver.h" + +namespace fcl +{ + +namespace detail +{ + +/// @brief CCD intersect kernel among primitives +template +class Intersect +{ + +public: + + /// @brief CCD intersect between one vertex and one face + /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 + /// p0 and p1 are points for vertex in time t0 and t1 + /// p_i returns the coordinate of the collision point + static bool intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between two edges + /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 + /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 + /// p_i returns the coordinate of the collision point + static bool intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between one vertex and one face, using additional filter + static bool intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between two edges, using additional filter + static bool intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between one vertex and and one edge + static bool intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& p1, + const Vector3& L); + + /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] + static bool intersect_Triangle_ODE_style( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Matrix3& R, + const Vector3& T, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Transform3& tf, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + +private: + + /// @brief Project function used in intersect_Triangle() + static int project6(const Vector3& ax, + const Vector3& p1, const Vector3& p2, const Vector3& p3, + const Vector3& q1, const Vector3& q2, const Vector3& q3); + + /// @brief Check whether one value is zero + static bool isZero(S v); + + /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction + static bool solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S& l, S& r, bool bVF, S coeffs[], Vector3* data = nullptr); + + /// @brief Check whether one point p is within triangle [a, b, c] + static bool insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p); + + /// @brief Check whether one point p is within a line segment [a, b] + static bool insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p); + + /// @brief Calculate the line segment papb that is the shortest route between + /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where + /// pa = p1 + mua (p2 - p1) + /// pb = p3 + mub (p4 - p3) + /// return FALSE if no solution exists. + static bool linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, + Vector3* pa, Vector3* pb, S* mua, S* mub); + + /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t + static bool checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S t); + + /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time + static bool checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S t, Vector3* q_i = nullptr); + + /// @brief Check whether a root for VE intersection is valid + static bool checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + S t); + + /// @brief Solve a square function for EE intersection (with interval restriction) + static bool solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + bool bVF, + S* ret); + + /// @brief Solve a square function for VE intersection (with interval restriction) + static bool solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp); + + /// @brief Compute the cubic coefficients for VF intersection + /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. + + static void computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S* a, S* b, S* c, S* d); + + /// @brief Compute the cubic coefficients for EE intersection + static void computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S* a, S* b, S* c, S* d); + + /// @brief Compute the cubic coefficients for VE intersection + static void computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + const Vector3& L, + S* a, S* b, S* c); + + /// @brief filter for intersection, works for both VF and EE + static bool intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1); + + /// @brief distance of point v to a plane n * x - t = 0 + static S distanceToPlane(const Vector3& n, S t, const Vector3& v); + + /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 + static bool sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t); + + /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to + static void clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, + const Vector3& t1, const Vector3& t2, const Vector3& t3, + const Vector3& tn, S to, + Vector3 clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false); + + /// @brief build a plane passed through triangle v1 v2 v3 + static bool buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t); + + /// @brief build a plane pass through edge v1 and v2, normal is tn + static bool buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t); + + /// @brief compute the points which has deepest penetration depth + static void computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points); + + /// @brief clip polygon by plane + static void clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points); + + /// @brief clip a line segment by plane + static void clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point); + + /// @brief compute the cdf(x) + static S gaussianCDF(S x); + + static constexpr S getEpsilon(); + static constexpr S getNearZeroThreshold(); + static constexpr S getCcdResolution(); + static constexpr unsigned int getMaxTriangleClips(); +}; + +using Intersectf = Intersect; +using Intersectd = Intersect; + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" + +#endif diff --git a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h index 0a2731fa1..21d7d094e 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h @@ -52,10 +52,11 @@ namespace detail //============================================================================== extern template -class FCL_EXPORT MeshCollisionTraversalNodeOBB; +class FCL_EXTERN_TEMPLATE_API MeshCollisionTraversalNodeOBB; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool initialize( MeshCollisionTraversalNodeOBB& node, const BVHModel>& model1, @@ -67,10 +68,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXPORT MeshCollisionTraversalNodeRSS; +class FCL_EXTERN_TEMPLATE_API MeshCollisionTraversalNodeRSS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool initialize( MeshCollisionTraversalNodeRSS& node, const BVHModel>& model1, @@ -82,10 +84,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXPORT MeshCollisionTraversalNodekIOS; +class FCL_EXTERN_TEMPLATE_API MeshCollisionTraversalNodekIOS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool initialize( MeshCollisionTraversalNodekIOS& node, const BVHModel>& model1, @@ -97,10 +100,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXPORT MeshCollisionTraversalNodeOBBRSS; +class FCL_EXTERN_TEMPLATE_API MeshCollisionTraversalNodeOBBRSS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool initialize( MeshCollisionTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h index 7e3d99335..6d0ec97f5 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h @@ -55,7 +55,7 @@ namespace detail /// @brief Traversal node for collision between two meshes template -class FCL_EXPORT MeshCollisionTraversalNode : public BVHCollisionTraversalNode +class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { public: @@ -81,7 +81,6 @@ class FCL_EXPORT MeshCollisionTraversalNode : public BVHCollisionTraversalNode -FCL_EXPORT bool initialize( MeshCollisionTraversalNode& node, BVHModel& model1, @@ -96,7 +95,7 @@ bool initialize( /// @brief Traversal node for collision between two meshes if their underlying /// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS) template -class FCL_EXPORT MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode> +class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode> { public: MeshCollisionTraversalNodeOBB(); @@ -125,7 +124,6 @@ using MeshCollisionTraversalNodeOBBd = MeshCollisionTraversalNodeOBB; /// @brief Initialize traversal node for collision between two meshes, /// specialized for OBB type template -FCL_EXPORT bool initialize( MeshCollisionTraversalNodeOBB& node, const BVHModel>& model1, @@ -136,7 +134,7 @@ bool initialize( CollisionResult& result); template -class FCL_EXPORT MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode> +class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode> { public: MeshCollisionTraversalNodeRSS(); @@ -167,7 +165,6 @@ using MeshCollisionTraversalNodeRSSd = MeshCollisionTraversalNodeRSS; /// @brief Initialize traversal node for collision between two meshes, /// specialized for RSS type template -FCL_EXPORT bool initialize( MeshCollisionTraversalNodeRSS& node, const BVHModel>& model1, @@ -178,11 +175,11 @@ bool initialize( CollisionResult& result); template -class FCL_EXPORT MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode> +class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode> { public: MeshCollisionTraversalNodekIOS(); - + bool BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; @@ -199,7 +196,6 @@ using MeshCollisionTraversalNodekIOSd = MeshCollisionTraversalNodekIOS; /// @brief Initialize traversal node for collision between two meshes, /// specialized for kIOS type template -FCL_EXPORT bool initialize( MeshCollisionTraversalNodekIOS& node, const BVHModel>& model1, @@ -210,11 +206,11 @@ bool initialize( CollisionResult& result); template -class FCL_EXPORT MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode> +class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode> { public: MeshCollisionTraversalNodeOBBRSS(); - + bool BVTesting(int b1, int b2) const; @@ -232,7 +228,6 @@ using MeshCollisionTraversalNodeOBBRSSd = MeshCollisionTraversalNodeOBBRSS -FCL_EXPORT bool initialize( MeshCollisionTraversalNodeOBBRSS& node, const BVHModel>& model1, @@ -243,7 +238,6 @@ bool initialize( CollisionResult& result); template -FCL_EXPORT void meshCollisionOrientedNodeLeafTesting( int b1, int b2, @@ -264,7 +258,6 @@ void meshCollisionOrientedNodeLeafTesting( CollisionResult& result); template -FCL_EXPORT void meshCollisionOrientedNodeLeafTesting( int b1, int b2, diff --git a/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node-inl.h index 39213d785..f3b263a32 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node-inl.h @@ -50,7 +50,7 @@ namespace detail //============================================================================== extern template -struct BVHContinuousCollisionPair; +struct FCL_EXTERN_TEMPLATE_API BVHContinuousCollisionPair; //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.h index 82425a51d..a9e7b0130 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.h @@ -48,7 +48,7 @@ namespace detail /// @brief Traversal node for continuous collision between BVH models template -struct FCL_EXPORT BVHContinuousCollisionPair +struct BVHContinuousCollisionPair { BVHContinuousCollisionPair(); @@ -66,7 +66,7 @@ struct FCL_EXPORT BVHContinuousCollisionPair /// @brief Traversal node for continuous collision between meshes template -class FCL_EXPORT MeshContinuousCollisionTraversalNode +class MeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode { public: @@ -101,7 +101,6 @@ class FCL_EXPORT MeshContinuousCollisionTraversalNode /// @brief Initialize traversal node for continuous collision detection between /// two meshes template -FCL_EXPORT bool initialize( MeshContinuousCollisionTraversalNode& node, const BVHModel& model1, diff --git a/include/fcl/narrowphase/detail/traversal/collision/mesh_shape_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/mesh_shape_collision_traversal_node.h index a06250a7a..5354b4018 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/mesh_shape_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/mesh_shape_collision_traversal_node.h @@ -49,7 +49,7 @@ namespace detail /// @brief Traversal node for collision between mesh and shape template -class FCL_EXPORT MeshShapeCollisionTraversalNode +class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode { public: @@ -66,7 +66,7 @@ class FCL_EXPORT MeshShapeCollisionTraversalNode Vector3* vertices; Triangle* tri_indices; - + S cost_density; const NarrowPhaseSolver* nsolver; @@ -75,7 +75,6 @@ class FCL_EXPORT MeshShapeCollisionTraversalNode /// @brief Initialize traversal node for collision between one mesh and one /// shape, given current object transform template -FCL_EXPORT bool initialize( MeshShapeCollisionTraversalNode& node, BVHModel& model1, @@ -88,7 +87,6 @@ bool initialize( bool use_refit = false, bool refit_bottomup = false); template -FCL_EXPORT void meshShapeCollisionOrientedNodeLeafTesting( int b1, int b2, @@ -107,7 +105,7 @@ void meshShapeCollisionOrientedNodeLeafTesting( /// @brief Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) template -class FCL_EXPORT MeshShapeCollisionTraversalNodeOBB +class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode< OBB, Shape, NarrowPhaseSolver> { @@ -123,7 +121,6 @@ class FCL_EXPORT MeshShapeCollisionTraversalNodeOBB /// @brief Initialize the traversal node for collision between one mesh and one /// shape, specialized for OBB type template -FCL_EXPORT bool initialize( MeshShapeCollisionTraversalNodeOBB& node, const BVHModel>& model1, @@ -135,7 +132,7 @@ bool initialize( CollisionResult& result); template -class FCL_EXPORT MeshShapeCollisionTraversalNodeRSS +class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode< RSS, Shape, NarrowPhaseSolver> { @@ -151,7 +148,6 @@ class FCL_EXPORT MeshShapeCollisionTraversalNodeRSS /// @brief Initialize the traversal node for collision between one mesh and one /// shape, specialized for RSS type template -FCL_EXPORT bool initialize( MeshShapeCollisionTraversalNodeRSS& node, const BVHModel>& model1, @@ -163,7 +159,7 @@ bool initialize( CollisionResult& result); template -class FCL_EXPORT MeshShapeCollisionTraversalNodekIOS +class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode< kIOS, Shape, NarrowPhaseSolver> { @@ -179,7 +175,6 @@ class FCL_EXPORT MeshShapeCollisionTraversalNodekIOS /// @brief Initialize the traversal node for collision between one mesh and one /// shape, specialized for kIOS type template -FCL_EXPORT bool initialize( MeshShapeCollisionTraversalNodekIOS& node, const BVHModel>& model1, @@ -191,7 +186,7 @@ bool initialize( CollisionResult& result); template -class FCL_EXPORT MeshShapeCollisionTraversalNodeOBBRSS +class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode< OBBRSS, Shape, NarrowPhaseSolver> { @@ -207,7 +202,6 @@ class FCL_EXPORT MeshShapeCollisionTraversalNodeOBBRSS /// @brief Initialize the traversal node for collision between one mesh and one /// shape, specialized for OBBRSS type template -FCL_EXPORT bool initialize( MeshShapeCollisionTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/include/fcl/narrowphase/detail/traversal/collision/shape_bvh_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/shape_bvh_collision_traversal_node.h index d66527960..fc2ca971f 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/shape_bvh_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/shape_bvh_collision_traversal_node.h @@ -49,7 +49,7 @@ namespace detail /// @brief Traversal node for collision between shape and BVH template -class FCL_EXPORT ShapeBVHCollisionTraversalNode +class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/collision/shape_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/shape_collision_traversal_node.h index 68394d1ce..575807f44 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/shape_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/shape_collision_traversal_node.h @@ -50,7 +50,7 @@ namespace detail /// @brief Traversal node for collision between two shapes template -class FCL_EXPORT ShapeCollisionTraversalNode +class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/collision/shape_mesh_collision_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/collision/shape_mesh_collision_traversal_node-inl.h index d4fbdfbe7..ed405f592 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/shape_mesh_collision_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/shape_mesh_collision_traversal_node-inl.h @@ -50,7 +50,6 @@ namespace detail //============================================================================== template -FCL_EXPORT ShapeMeshCollisionTraversalNode::ShapeMeshCollisionTraversalNode() : ShapeBVHCollisionTraversalNode() { @@ -62,7 +61,6 @@ ShapeMeshCollisionTraversalNode::ShapeMeshCollisio //============================================================================== template -FCL_EXPORT void ShapeMeshCollisionTraversalNode::leafTesting(int b1, int b2) const { FCL_UNUSED(b1); @@ -131,7 +129,6 @@ void ShapeMeshCollisionTraversalNode::leafTesting( //============================================================================== template -FCL_EXPORT bool ShapeMeshCollisionTraversalNode::canStop() const { return this->request.isSatisfied(*(this->result)); @@ -139,7 +136,6 @@ bool ShapeMeshCollisionTraversalNode::canStop() co //============================================================================== template -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNode& node, const Shape& model1, @@ -195,14 +191,12 @@ bool initialize( //============================================================================== template -FCL_EXPORT ShapeMeshCollisionTraversalNodeOBB::ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode, NarrowPhaseSolver>() { } //============================================================================== template -FCL_EXPORT bool ShapeMeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const { FCL_UNUSED(b1); @@ -213,7 +207,6 @@ bool ShapeMeshCollisionTraversalNodeOBB::BVTesting(int //============================================================================== template -FCL_EXPORT void ShapeMeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const { detail::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, @@ -224,14 +217,12 @@ void ShapeMeshCollisionTraversalNodeOBB::leafTesting(i //============================================================================== template -FCL_EXPORT ShapeMeshCollisionTraversalNodeRSS::ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode, NarrowPhaseSolver>() { } //============================================================================== template -FCL_EXPORT bool ShapeMeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const { FCL_UNUSED(b1); @@ -242,7 +233,6 @@ bool ShapeMeshCollisionTraversalNodeRSS::BVTesting(int //============================================================================== template -FCL_EXPORT void ShapeMeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const { detail::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, @@ -253,14 +243,12 @@ void ShapeMeshCollisionTraversalNodeRSS::leafTesting(i //============================================================================== template -FCL_EXPORT ShapeMeshCollisionTraversalNodekIOS::ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode, NarrowPhaseSolver>() { } //============================================================================== template -FCL_EXPORT bool ShapeMeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const { FCL_UNUSED(b1); @@ -271,7 +259,6 @@ bool ShapeMeshCollisionTraversalNodekIOS::BVTesting(in //============================================================================== template -FCL_EXPORT void ShapeMeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const { detail::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, @@ -282,14 +269,12 @@ void ShapeMeshCollisionTraversalNodekIOS::leafTesting( //============================================================================== template -FCL_EXPORT ShapeMeshCollisionTraversalNodeOBBRSS::ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode, NarrowPhaseSolver>() { } //============================================================================== template -FCL_EXPORT bool ShapeMeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const { FCL_UNUSED(b1); @@ -300,7 +285,6 @@ bool ShapeMeshCollisionTraversalNodeOBBRSS::BVTesting( //============================================================================== template -FCL_EXPORT void ShapeMeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const { detail::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices, @@ -341,7 +325,6 @@ static bool setupShapeMeshCollisionOrientedNode(OrientedNode -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNodeOBB& node, const Shape& model1, @@ -358,7 +341,6 @@ bool initialize( //============================================================================== template -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNodeRSS& node, const Shape& model1, @@ -375,7 +357,6 @@ bool initialize( //============================================================================== template -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNodekIOS& node, const Shape& model1, @@ -392,7 +373,6 @@ bool initialize( //============================================================================== template -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNodeOBBRSS& node, const Shape& model1, diff --git a/include/fcl/narrowphase/detail/traversal/collision/shape_mesh_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/shape_mesh_collision_traversal_node.h index 5ebcc0308..1514a5eec 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/shape_mesh_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/shape_mesh_collision_traversal_node.h @@ -49,7 +49,7 @@ namespace detail /// @brief Traversal node for collision between shape and mesh template -class FCL_EXPORT ShapeMeshCollisionTraversalNode +class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode { public: @@ -74,7 +74,6 @@ class FCL_EXPORT ShapeMeshCollisionTraversalNode /// @brief Initialize traversal node for collision between one mesh and one /// shape, given current object transform template -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNode& node, const Shape& model1, @@ -88,7 +87,7 @@ bool initialize( /// @brief Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS) template -class FCL_EXPORT ShapeMeshCollisionTraversalNodeOBB +class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode< Shape, OBB, NarrowPhaseSolver> { @@ -103,7 +102,6 @@ class FCL_EXPORT ShapeMeshCollisionTraversalNodeOBB /// @brief Initialize the traversal node for collision between one mesh and one /// shape, specialized for OBB type template -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNodeOBB& node, const Shape& model1, @@ -115,7 +113,7 @@ bool initialize( CollisionResult& result); template -class FCL_EXPORT ShapeMeshCollisionTraversalNodeRSS +class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode, NarrowPhaseSolver> { public: @@ -130,7 +128,6 @@ class FCL_EXPORT ShapeMeshCollisionTraversalNodeRSS /// @brief Initialize the traversal node for collision between one mesh and one /// shape, specialized for RSS type template -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNodeRSS& node, const Shape& model1, @@ -142,7 +139,7 @@ bool initialize( CollisionResult& result); template -class FCL_EXPORT ShapeMeshCollisionTraversalNodekIOS +class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode, NarrowPhaseSolver> { public: @@ -157,7 +154,6 @@ class FCL_EXPORT ShapeMeshCollisionTraversalNodekIOS /// @brief Initialize the traversal node for collision between one mesh and one /// shape, specialized for kIOS type template -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNodekIOS& node, const Shape& model1, @@ -169,7 +165,7 @@ bool initialize( CollisionResult& result); template -class FCL_EXPORT ShapeMeshCollisionTraversalNodeOBBRSS +class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode, NarrowPhaseSolver> { public: @@ -184,7 +180,6 @@ class FCL_EXPORT ShapeMeshCollisionTraversalNodeOBBRSS /// @brief Initialize the traversal node for collision between one mesh and one /// shape, specialized for OBBRSS type template -FCL_EXPORT bool initialize( ShapeMeshCollisionTraversalNodeOBBRSS& node, const Shape& model1, diff --git a/include/fcl/narrowphase/detail/traversal/collision_node-inl.h b/include/fcl/narrowphase/detail/traversal/collision_node-inl.h index 73d7a7293..305931c1b 100644 --- a/include/fcl/narrowphase/detail/traversal/collision_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision_node-inl.h @@ -49,22 +49,27 @@ namespace detail //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list); //============================================================================== diff --git a/include/fcl/narrowphase/detail/traversal/collision_node.h b/include/fcl/narrowphase/detail/traversal/collision_node.h index c00a1a1b6..d15f4ee16 100644 --- a/include/fcl/narrowphase/detail/traversal/collision_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision_node.h @@ -53,27 +53,22 @@ namespace detail /// @brief collision on collision traversal node; can use front list to accelerate template -FCL_EXPORT void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = nullptr); /// @brief self collision on collision traversal node; can use front list to accelerate template -FCL_EXPORT void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = nullptr); /// @brief distance computation on distance traversal node; can use front list to accelerate template -FCL_EXPORT void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = nullptr, int qsize = 2); /// @brief special collision on OBB traversal node template -FCL_EXPORT void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list = nullptr); /// @brief special collision on RSS traversal node template -FCL_EXPORT void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list = nullptr); } // namespace detail diff --git a/include/fcl/narrowphase/detail/traversal/distance/bvh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/bvh_distance_traversal_node.h index d56cbe2c9..5310bdd6b 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/bvh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/bvh_distance_traversal_node.h @@ -50,7 +50,7 @@ namespace detail /// @brief Traversal node for distance computation between BVH models template -class FCL_EXPORT BVHDistanceTraversalNode +class BVHDistanceTraversalNode : public DistanceTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/distance/bvh_shape_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/bvh_shape_distance_traversal_node.h index 50f4fc21c..8231fc94b 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/bvh_shape_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/bvh_shape_distance_traversal_node.h @@ -49,7 +49,7 @@ namespace detail /// @brief Traversal node for distance computation between BVH and shape template -class FCL_EXPORT BVHShapeDistanceTraversalNode +class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data-inl.h b/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data-inl.h index 9c567ace9..26e0be7a5 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data-inl.h @@ -48,7 +48,7 @@ namespace detail //============================================================================== extern template -struct ConservativeAdvancementStackData; +struct FCL_EXTERN_TEMPLATE_API ConservativeAdvancementStackData; //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.h b/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.h index fb95a940d..b372f8436 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.h +++ b/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.h @@ -47,7 +47,7 @@ namespace detail { template -struct FCL_EXPORT ConservativeAdvancementStackData +struct ConservativeAdvancementStackData { ConservativeAdvancementStackData( const Vector3& P1_, diff --git a/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base-inl.h b/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base-inl.h index 5fbeb5e0c..5cd460c0a 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base-inl.h @@ -50,7 +50,7 @@ namespace detail //============================================================================== extern template -class FCL_EXPORT DistanceTraversalNodeBase; +class FCL_EXTERN_TEMPLATE_API DistanceTraversalNodeBase; //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base.h b/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base.h index e4a69a26c..72437edd0 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base.h +++ b/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base.h @@ -50,7 +50,7 @@ namespace detail /// @brief Node structure encoding the information required for distance traversal. template -class FCL_EXPORT DistanceTraversalNodeBase : public TraversalNodeBase +class DistanceTraversalNodeBase : public TraversalNodeBase { public: DistanceTraversalNodeBase(); diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h index 44ddde5fe..0c5512672 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h @@ -51,10 +51,11 @@ namespace detail //============================================================================== extern template -class FCL_EXPORT MeshConservativeAdvancementTraversalNodeRSS; +class FCL_EXTERN_TEMPLATE_API MeshConservativeAdvancementTraversalNodeRSS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool initialize( MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel>& model1, @@ -65,10 +66,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXPORT MeshConservativeAdvancementTraversalNodeOBBRSS; +class FCL_EXTERN_TEMPLATE_API MeshConservativeAdvancementTraversalNodeOBBRSS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool initialize( MeshConservativeAdvancementTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h index be72620e6..a1cbfcd68 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h @@ -50,7 +50,7 @@ namespace detail /// @brief continuous collision node using conservative advancement. when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration template -class FCL_EXPORT MeshConservativeAdvancementTraversalNode +class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode { public: @@ -69,9 +69,9 @@ class FCL_EXPORT MeshConservativeAdvancementTraversalNode bool canStop(S c) const; mutable S min_distance; - + mutable Vector3 closest_p1, closest_p2; - + mutable int last_tri_id1, last_tri_id2; /// @brief CA controlling variable: early stop for the early iterations of CA @@ -97,7 +97,6 @@ class FCL_EXPORT MeshConservativeAdvancementTraversalNode /// @brief Initialize traversal node for conservative advancement computation /// between two meshes, given the current transforms template -FCL_EXPORT bool initialize( MeshConservativeAdvancementTraversalNode& node, BVHModel& model1, @@ -109,7 +108,7 @@ bool initialize( bool refit_bottomup = false); template -class FCL_EXPORT MeshConservativeAdvancementTraversalNodeRSS +class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode> { public: @@ -150,7 +149,6 @@ using MeshConservativeAdvancementTraversalNodeRSSd = MeshConservativeAdvancement /// @brief Initialize traversal node for conservative advancement computation /// between two meshes, given the current transforms, specialized for RSS template -FCL_EXPORT bool initialize( MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel>& model1, @@ -160,7 +158,7 @@ bool initialize( S w = 1); template -class FCL_EXPORT MeshConservativeAdvancementTraversalNodeOBBRSS +class MeshConservativeAdvancementTraversalNodeOBBRSS : public MeshConservativeAdvancementTraversalNode> { public: @@ -199,7 +197,6 @@ using MeshConservativeAdvancementTraversalNodeOBBRSSf = MeshConservativeAdvancem using MeshConservativeAdvancementTraversalNodeOBBRSSd = MeshConservativeAdvancementTraversalNodeOBBRSS; template -FCL_EXPORT bool initialize( MeshConservativeAdvancementTraversalNodeOBBRSS& node, const BVHModel>& model1, @@ -209,11 +206,9 @@ bool initialize( S w = 1); template -FCL_EXPORT const Vector3 getBVAxis(const BV& bv, int i); template -FCL_EXPORT bool meshConservativeAdvancementTraversalNodeCanStop( typename BV::S c, typename BV::S min_distance, @@ -228,7 +223,6 @@ bool meshConservativeAdvancementTraversalNodeCanStop( typename BV::S& delta_t); template -FCL_EXPORT bool meshConservativeAdvancementOrientedNodeCanStop( typename BV::S c, typename BV::S min_distance, @@ -243,7 +237,6 @@ bool meshConservativeAdvancementOrientedNodeCanStop( typename BV::S& delta_t); template -FCL_EXPORT void meshConservativeAdvancementOrientedNodeLeafTesting( int b1, int b2, diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node-inl.h index 558149dc9..012990257 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node-inl.h @@ -48,10 +48,11 @@ namespace detail //============================================================================== extern template -class FCL_EXPORT MeshDistanceTraversalNodeRSS; +class FCL_EXTERN_TEMPLATE_API MeshDistanceTraversalNodeRSS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool initialize( MeshDistanceTraversalNodeRSS& node, const BVHModel>& model1, @@ -63,10 +64,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXPORT MeshDistanceTraversalNodekIOS; +class FCL_EXTERN_TEMPLATE_API MeshDistanceTraversalNodekIOS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool initialize( MeshDistanceTraversalNodekIOS& node, const BVHModel>& model1, @@ -78,10 +80,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXPORT MeshDistanceTraversalNodeOBBRSS; +class FCL_EXTERN_TEMPLATE_API MeshDistanceTraversalNodeOBBRSS; //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API bool initialize( MeshDistanceTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h index 92f44617e..acd6a2a7e 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h @@ -52,7 +52,7 @@ namespace detail /// @brief Traversal node for distance computation between two meshes template -class FCL_EXPORT MeshDistanceTraversalNode : public BVHDistanceTraversalNode +class MeshDistanceTraversalNode : public BVHDistanceTraversalNode { public: @@ -80,7 +80,6 @@ class FCL_EXPORT MeshDistanceTraversalNode : public BVHDistanceTraversalNode /// @brief Initialize traversal node for distance computation between two /// meshes, given the current transforms template -FCL_EXPORT bool initialize( MeshDistanceTraversalNode& node, BVHModel& model1, @@ -93,7 +92,7 @@ bool initialize( /// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS) template -class FCL_EXPORT MeshDistanceTraversalNodeRSS +class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode> { public: @@ -123,7 +122,6 @@ using MeshDistanceTraversalNodeRSSd = MeshDistanceTraversalNodeRSS; /// @brief Initialize traversal node for distance computation between two /// meshes, specialized for RSS type template -FCL_EXPORT bool initialize( MeshDistanceTraversalNodeRSS& node, const BVHModel>& model1, @@ -134,14 +132,14 @@ bool initialize( DistanceResult& result); template -class FCL_EXPORT MeshDistanceTraversalNodekIOS +class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode> { public: MeshDistanceTraversalNodekIOS(); void preprocess(); - + void postprocess(); S BVTesting(int b1, int b2) const @@ -164,7 +162,6 @@ using MeshDistanceTraversalNodekIOSd = MeshDistanceTraversalNodekIOS; /// @brief Initialize traversal node for distance computation between two /// meshes, specialized for kIOS type template -FCL_EXPORT bool initialize( MeshDistanceTraversalNodekIOS& node, const BVHModel>& model1, @@ -175,7 +172,7 @@ bool initialize( DistanceResult& result); template -class FCL_EXPORT MeshDistanceTraversalNodeOBBRSS +class MeshDistanceTraversalNodeOBBRSS : public MeshDistanceTraversalNode> { public: @@ -205,7 +202,6 @@ using MeshDistanceTraversalNodeOBBRSSd = MeshDistanceTraversalNodeOBBRSS /// @brief Initialize traversal node for distance computation between two /// meshes, specialized for OBBRSS type template -FCL_EXPORT bool initialize( MeshDistanceTraversalNodeOBBRSS& node, const BVHModel>& model1, @@ -216,7 +212,6 @@ bool initialize( DistanceResult& result); template -FCL_DEPRECATED_EXPORT void meshDistanceOrientedNodeLeafTesting( int b1, int b2, @@ -234,7 +229,6 @@ void meshDistanceOrientedNodeLeafTesting( DistanceResult& result); template -FCL_EXPORT void meshDistanceOrientedNodeLeafTesting( int b1, int b2, @@ -251,7 +245,6 @@ void meshDistanceOrientedNodeLeafTesting( DistanceResult& result); template -FCL_EXPORT void distancePreprocessOrientedNode( const BVHModel* model1, const BVHModel* model2, @@ -267,7 +260,6 @@ void distancePreprocessOrientedNode( DistanceResult& result); template -FCL_EXPORT void distancePreprocessOrientedNode( const BVHModel* model1, const BVHModel* model2, @@ -282,7 +274,6 @@ void distancePreprocessOrientedNode( DistanceResult& result); template -FCL_EXPORT void distancePostprocessOrientedNode( const BVHModel* model1, const BVHModel* model2, diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h index a9f8d7597..20b7e232b 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h @@ -49,7 +49,7 @@ namespace detail /// @brief Traversal node for conservative advancement computation between BVH and shape template -class FCL_EXPORT MeshShapeConservativeAdvancementTraversalNode +class MeshShapeConservativeAdvancementTraversalNode : public MeshShapeDistanceTraversalNode { public: @@ -72,7 +72,7 @@ class FCL_EXPORT MeshShapeConservativeAdvancementTraversalNode mutable Vector3 closest_p1, closest_p2; mutable int last_tri_id; - + /// @brief CA controlling variable: early stop for the early iterations of CA S w; @@ -140,7 +140,7 @@ bool meshShapeConservativeAdvancementOrientedNodeCanStop( typename BV::S& delta_t); template -class FCL_EXPORT MeshShapeConservativeAdvancementTraversalNodeRSS +class MeshShapeConservativeAdvancementTraversalNodeRSS : public MeshShapeConservativeAdvancementTraversalNode< RSS, Shape, NarrowPhaseSolver> { @@ -168,7 +168,7 @@ bool initialize( typename Shape::S w = 1); template -class FCL_EXPORT MeshShapeConservativeAdvancementTraversalNodeOBBRSS : +class MeshShapeConservativeAdvancementTraversalNodeOBBRSS : public MeshShapeConservativeAdvancementTraversalNode< OBBRSS, Shape, NarrowPhaseSolver> { diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h index 0d40e2ef5..79c5fde91 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h @@ -49,9 +49,9 @@ namespace detail /// @brief Traversal node for distance between mesh and shape template -class FCL_EXPORT MeshShapeDistanceTraversalNode +class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode -{ +{ public: using S = typename BV::S; @@ -69,7 +69,7 @@ class FCL_EXPORT MeshShapeDistanceTraversalNode S rel_err; S abs_err; - + const NarrowPhaseSolver* nsolver; }; @@ -118,7 +118,7 @@ bool initialize( /// @brief Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS, OBBRSS, kIOS) template -class FCL_EXPORT MeshShapeDistanceTraversalNodeRSS +class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode< RSS, Shape, NarrowPhaseSolver> { @@ -148,7 +148,7 @@ bool initialize( DistanceResult& result); template -class FCL_EXPORT MeshShapeDistanceTraversalNodekIOS +class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode, Shape, NarrowPhaseSolver> { public: @@ -177,7 +177,7 @@ bool initialize( DistanceResult& result); template -class FCL_EXPORT MeshShapeDistanceTraversalNodeOBBRSS +class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode, Shape, NarrowPhaseSolver> { public: @@ -192,7 +192,7 @@ class FCL_EXPORT MeshShapeDistanceTraversalNodeOBBRSS S BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; - + }; /// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h index 53176cfb8..05ae2c10b 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h @@ -50,7 +50,7 @@ namespace detail /// @brief Traversal node for distance computation between shape and BVH template -class FCL_EXPORT ShapeBVHDistanceTraversalNode +class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase { public: @@ -74,7 +74,7 @@ class FCL_EXPORT ShapeBVHDistanceTraversalNode const Shape* model1; const BVHModel* model2; BV model1_bv; - + mutable int num_bv_tests; mutable int num_leaf_tests; mutable S query_time_seconds; diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node.h index 525df7215..3e92008e2 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_conservative_advancement_traversal_node.h @@ -47,7 +47,7 @@ namespace detail { template -class FCL_EXPORT ShapeConservativeAdvancementTraversalNode +class ShapeConservativeAdvancementTraversalNode : public ShapeDistanceTraversalNode { public: diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_distance_traversal_node.h index bfd1e1f64..ad7499257 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_distance_traversal_node.h @@ -49,7 +49,7 @@ namespace detail /// @brief Traversal node for distance between two shapes template -class FCL_EXPORT ShapeDistanceTraversalNode +class ShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h index bf54940a5..01b599527 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h @@ -49,7 +49,7 @@ namespace detail { template -class FCL_EXPORT ShapeMeshConservativeAdvancementTraversalNode +class ShapeMeshConservativeAdvancementTraversalNode : public ShapeMeshDistanceTraversalNode { public: @@ -72,7 +72,7 @@ class FCL_EXPORT ShapeMeshConservativeAdvancementTraversalNode mutable Vector3 closest_p1, closest_p2; mutable int last_tri_id; - + /// @brief CA controlling variable: early stop for the early iterations of CA S w; @@ -103,7 +103,7 @@ bool initialize( bool refit_bottomup = false); template -class FCL_EXPORT ShapeMeshConservativeAdvancementTraversalNodeRSS +class ShapeMeshConservativeAdvancementTraversalNodeRSS : public ShapeMeshConservativeAdvancementTraversalNode< Shape, RSS, NarrowPhaseSolver> { @@ -130,7 +130,7 @@ bool initialize( typename Shape::S w = 1); template -class FCL_EXPORT ShapeMeshConservativeAdvancementTraversalNodeOBBRSS +class ShapeMeshConservativeAdvancementTraversalNodeOBBRSS : public ShapeMeshConservativeAdvancementTraversalNode< Shape, OBBRSS, NarrowPhaseSolver> { diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node-inl.h index cdf8c8d00..c059ed10e 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node-inl.h @@ -108,7 +108,6 @@ bool ShapeMeshDistanceTraversalNode::canStop(S c) //============================================================================== template -FCL_EXPORT bool initialize( ShapeMeshDistanceTraversalNode& node, const Shape& model1, @@ -349,7 +348,6 @@ static bool setupShapeMeshDistanceOrientedNode(OrientedNode -FCL_EXPORT bool initialize(ShapeMeshDistanceTraversalNodeRSS& node, const Shape& model1, const Transform3& tf1, const BVHModel>& model2, const Transform3& tf2, @@ -362,7 +360,6 @@ bool initialize(ShapeMeshDistanceTraversalNodeRSS& nod //============================================================================== template -FCL_EXPORT bool initialize(ShapeMeshDistanceTraversalNodekIOS& node, const Shape& model1, const Transform3& tf1, const BVHModel>& model2, const Transform3& tf2, @@ -375,7 +372,6 @@ bool initialize(ShapeMeshDistanceTraversalNodekIOS& no //============================================================================== template -FCL_EXPORT bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS& node, const Shape& model1, const Transform3& tf1, const BVHModel>& model2, const Transform3& tf2, diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h index 438f2053d..a90c0077f 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h @@ -49,9 +49,9 @@ namespace detail /// @brief Traversal node for distance between shape and mesh template -class FCL_EXPORT ShapeMeshDistanceTraversalNode +class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode -{ +{ public: using S = typename BV::S; @@ -69,7 +69,7 @@ class FCL_EXPORT ShapeMeshDistanceTraversalNode S rel_err; S abs_err; - + const NarrowPhaseSolver* nsolver; }; @@ -89,7 +89,7 @@ bool initialize( bool refit_bottomup = false); template -class FCL_EXPORT ShapeMeshDistanceTraversalNodeRSS +class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode< Shape, RSS, NarrowPhaseSolver> { @@ -123,7 +123,7 @@ bool initialize( DistanceResult& result); template -class FCL_EXPORT ShapeMeshDistanceTraversalNodekIOS +class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode< Shape, kIOS, NarrowPhaseSolver> { @@ -140,7 +140,7 @@ class FCL_EXPORT ShapeMeshDistanceTraversalNodekIOS S BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; - + }; /// @brief Initialize traversal node for distance computation between one shape @@ -157,7 +157,7 @@ bool initialize( DistanceResult& result); template -class FCL_EXPORT ShapeMeshDistanceTraversalNodeOBBRSS +class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode< Shape, OBBRSS, NarrowPhaseSolver> { @@ -174,7 +174,7 @@ class FCL_EXPORT ShapeMeshDistanceTraversalNodeOBBRSS S BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; - + }; /// @brief Initialize traversal node for distance computation between one shape diff --git a/include/fcl/narrowphase/detail/traversal/octree/collision/mesh_octree_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/collision/mesh_octree_collision_traversal_node.h index 2792dd204..f060a66d6 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/collision/mesh_octree_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/collision/mesh_octree_collision_traversal_node.h @@ -56,7 +56,7 @@ namespace detail /// @brief Traversal node for mesh-octree collision template -class FCL_EXPORT MeshOcTreeCollisionTraversalNode +class MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/collision/octree_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/collision/octree_collision_traversal_node.h index b12245948..7692db9f8 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/collision/octree_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/collision/octree_collision_traversal_node.h @@ -55,7 +55,7 @@ namespace detail /// @brief Traversal node for octree collision template -class FCL_EXPORT OcTreeCollisionTraversalNode +class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/collision/octree_mesh_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/collision/octree_mesh_collision_traversal_node.h index 304a9cb1c..7c16d8f27 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/collision/octree_mesh_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/collision/octree_mesh_collision_traversal_node.h @@ -56,7 +56,7 @@ namespace detail /// @brief Traversal node for octree-mesh collision template -class FCL_EXPORT OcTreeMeshCollisionTraversalNode +class OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/collision/octree_shape_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/collision/octree_shape_collision_traversal_node.h index 93b53abf2..36962baa8 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/collision/octree_shape_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/collision/octree_shape_collision_traversal_node.h @@ -56,7 +56,7 @@ namespace detail /// @brief Traversal node for octree-shape collision template -class FCL_EXPORT OcTreeShapeCollisionTraversalNode +class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/collision/shape_octree_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/collision/shape_octree_collision_traversal_node.h index 147de063c..998b152aa 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/collision/shape_octree_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/collision/shape_octree_collision_traversal_node.h @@ -56,7 +56,7 @@ namespace detail /// @brief Traversal node for shape-octree collision template -class FCL_EXPORT ShapeOcTreeCollisionTraversalNode +class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/distance/mesh_octree_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/distance/mesh_octree_distance_traversal_node.h index b299e786c..cafb22e9f 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/distance/mesh_octree_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/distance/mesh_octree_distance_traversal_node.h @@ -56,7 +56,7 @@ namespace detail /// @brief Traversal node for mesh-octree distance template -class FCL_EXPORT MeshOcTreeDistanceTraversalNode +class MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/distance/octree_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/distance/octree_distance_traversal_node.h index 2df84244e..3fc223ccf 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/distance/octree_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/distance/octree_distance_traversal_node.h @@ -55,7 +55,7 @@ namespace detail /// @brief Traversal node for octree distance template -class FCL_EXPORT OcTreeDistanceTraversalNode +class OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/distance/octree_mesh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/distance/octree_mesh_distance_traversal_node.h index 5ac1cc28b..c1fbfeb82 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/distance/octree_mesh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/distance/octree_mesh_distance_traversal_node.h @@ -56,7 +56,7 @@ namespace detail /// @brief Traversal node for octree-mesh distance template -class FCL_EXPORT OcTreeMeshDistanceTraversalNode +class OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/distance/octree_shape_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/distance/octree_shape_distance_traversal_node.h index 482f5f87f..b9bebf56e 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/distance/octree_shape_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/distance/octree_shape_distance_traversal_node.h @@ -56,7 +56,7 @@ namespace detail /// @brief Traversal node for octree-shape distance template -class FCL_EXPORT OcTreeShapeDistanceTraversalNode +class OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/distance/shape_octree_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/octree/distance/shape_octree_distance_traversal_node.h index 02b3547b0..06d2da686 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/distance/shape_octree_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/octree/distance/shape_octree_distance_traversal_node.h @@ -56,7 +56,7 @@ namespace detail /// @brief Traversal node for shape-octree distance template -class FCL_EXPORT ShapeOcTreeDistanceTraversalNode +class ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase { public: diff --git a/include/fcl/narrowphase/detail/traversal/octree/octree_solver.h b/include/fcl/narrowphase/detail/traversal/octree/octree_solver.h index 415974254..1446012ad 100644 --- a/include/fcl/narrowphase/detail/traversal/octree/octree_solver.h +++ b/include/fcl/narrowphase/detail/traversal/octree/octree_solver.h @@ -56,7 +56,7 @@ namespace detail /// @brief Algorithms for collision related with octree template -class FCL_EXPORT OcTreeSolver +class OcTreeSolver { private: diff --git a/include/fcl/narrowphase/detail/traversal/traversal_node_base-inl.h b/include/fcl/narrowphase/detail/traversal/traversal_node_base-inl.h index 96d849b98..928071e24 100644 --- a/include/fcl/narrowphase/detail/traversal/traversal_node_base-inl.h +++ b/include/fcl/narrowphase/detail/traversal/traversal_node_base-inl.h @@ -50,7 +50,7 @@ namespace detail //============================================================================== extern template -class FCL_EXPORT TraversalNodeBase; +class FCL_EXTERN_TEMPLATE_API TraversalNodeBase; //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/traversal_node_base.h b/include/fcl/narrowphase/detail/traversal/traversal_node_base.h index 2f3b1877e..93cd7f096 100644 --- a/include/fcl/narrowphase/detail/traversal/traversal_node_base.h +++ b/include/fcl/narrowphase/detail/traversal/traversal_node_base.h @@ -48,16 +48,16 @@ namespace detail /// @brief Node structure encoding the information required for traversal. template -class FCL_EXPORT TraversalNodeBase +class TraversalNodeBase { public: virtual ~TraversalNodeBase(); virtual void preprocess(); - + virtual void postprocess(); - /// @brief Whether b is a leaf node in the first BVH tree + /// @brief Whether b is a leaf node in the first BVH tree virtual bool isFirstNodeLeaf(int b) const; /// @brief Whether b is a leaf node in the second BVH tree diff --git a/include/fcl/narrowphase/detail/traversal/traversal_recurse-inl.h b/include/fcl/narrowphase/detail/traversal/traversal_recurse-inl.h index 1c46c1c41..7d81b0f7c 100644 --- a/include/fcl/narrowphase/detail/traversal/traversal_recurse-inl.h +++ b/include/fcl/narrowphase/detail/traversal/traversal_recurse-inl.h @@ -52,35 +52,41 @@ namespace detail //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize); //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list); //============================================================================== template -FCL_EXPORT void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); @@ -130,7 +136,6 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFr //============================================================================== template -FCL_EXPORT void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); @@ -216,7 +221,6 @@ void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, co //============================================================================== template -FCL_EXPORT void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list) { FCL_UNUSED(node); @@ -234,7 +238,6 @@ void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, co * Make sure node is set correctly so that the first and second tree are the same */ template -FCL_EXPORT void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list) { bool l = node->isFirstNodeLeaf(b); @@ -255,7 +258,6 @@ void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontLi //============================================================================== template -FCL_EXPORT void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list) { bool l1 = node->isFirstNodeLeaf(b1); @@ -318,7 +320,7 @@ void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFron //============================================================================== /** @brief Bounding volume test structure */ template -struct FCL_EXPORT BVT +struct BVT { /** @brief distance between bvs */ S d; @@ -330,7 +332,7 @@ struct FCL_EXPORT BVT //============================================================================== /** @brief Comparer between two BVT */ template -struct FCL_EXPORT BVT_Comparer +struct BVT_Comparer { bool operator() (const BVT& lhs, const BVT& rhs) const { @@ -340,7 +342,7 @@ struct FCL_EXPORT BVT_Comparer //============================================================================== template -struct FCL_EXPORT BVTQ +struct BVTQ { BVTQ() : qsize(2) {} @@ -382,7 +384,6 @@ struct FCL_EXPORT BVTQ //============================================================================== template -FCL_EXPORT void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize) { BVTQ bvtq; @@ -461,7 +462,6 @@ void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BV //============================================================================== template -FCL_EXPORT void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list) { BVHFrontList::iterator front_iter; diff --git a/include/fcl/narrowphase/detail/traversal/traversal_recurse.h b/include/fcl/narrowphase/detail/traversal/traversal_recurse.h index d586c7e8c..cb9316f08 100644 --- a/include/fcl/narrowphase/detail/traversal/traversal_recurse.h +++ b/include/fcl/narrowphase/detail/traversal/traversal_recurse.h @@ -52,37 +52,30 @@ namespace detail /// @brief Recurse function for collision template -FCL_EXPORT void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); /// @brief Recurse function for collision, specialized for OBB type template -FCL_EXPORT void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); /// @brief Recurse function for collision, specialized for RSS type template -FCL_EXPORT void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); /// @brief Recurse function for self collision. Make sure node is set correctly so that the first and second tree are the same template -FCL_EXPORT void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list); /// @brief Recurse function for distance template -FCL_EXPORT void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); /// @brief Recurse function for distance, using queue acceleration template -FCL_EXPORT void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize); /// @brief Recurse function for front list propagation template -FCL_EXPORT void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list); } // namespace detail diff --git a/include/fcl/narrowphase/distance-inl.h b/include/fcl/narrowphase/distance-inl.h index 115b71027..f0227d445 100644 --- a/include/fcl/narrowphase/distance-inl.h +++ b/include/fcl/narrowphase/distance-inl.h @@ -47,6 +47,7 @@ namespace fcl //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double distance( const CollisionObject* o1, const CollisionObject* o2, @@ -55,6 +56,7 @@ double distance( //============================================================================== extern template +FCL_EXTERN_TEMPLATE_API double distance( const CollisionGeometry* o1, const Transform3& tf1, const CollisionGeometry* o2, const Transform3& tf2, diff --git a/include/fcl/narrowphase/distance.h b/include/fcl/narrowphase/distance.h index 7629d264f..9d77e52d0 100644 --- a/include/fcl/narrowphase/distance.h +++ b/include/fcl/narrowphase/distance.h @@ -46,16 +46,14 @@ namespace fcl { -/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. +/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. /// Return value is the minimum distance generated between the two objects. template -FCL_EXPORT S distance( const CollisionObject* o1, const CollisionObject* o2, const DistanceRequest& request, DistanceResult& result); template -FCL_EXPORT S distance( const CollisionGeometry* o1, const Transform3& tf1, const CollisionGeometry* o2, const Transform3& tf2, diff --git a/include/fcl/narrowphase/distance_request-inl.h b/include/fcl/narrowphase/distance_request-inl.h index 1c63dea33..18d928cac 100644 --- a/include/fcl/narrowphase/distance_request-inl.h +++ b/include/fcl/narrowphase/distance_request-inl.h @@ -47,7 +47,7 @@ namespace fcl //============================================================================== extern template -struct DistanceRequest; +struct FCL_EXTERN_TEMPLATE_API DistanceRequest; //============================================================================== template diff --git a/include/fcl/narrowphase/distance_request.h b/include/fcl/narrowphase/distance_request.h index c157699be..8dc92a15d 100644 --- a/include/fcl/narrowphase/distance_request.h +++ b/include/fcl/narrowphase/distance_request.h @@ -49,7 +49,7 @@ struct DistanceResult; /// @brief request to the distance computation template -struct FCL_EXPORT DistanceRequest +struct DistanceRequest { /// @brief whether to return the nearest points bool enable_nearest_points; diff --git a/include/fcl/narrowphase/distance_result-inl.h b/include/fcl/narrowphase/distance_result-inl.h index 424cffdf7..40a26f789 100644 --- a/include/fcl/narrowphase/distance_result-inl.h +++ b/include/fcl/narrowphase/distance_result-inl.h @@ -45,11 +45,10 @@ namespace fcl //============================================================================== extern template -struct DistanceResult; +struct FCL_EXTERN_TEMPLATE_API DistanceResult; //============================================================================== template -FCL_EXPORT DistanceResult::DistanceResult(S min_distance_) : min_distance(min_distance_), o1(nullptr), @@ -62,7 +61,6 @@ DistanceResult::DistanceResult(S min_distance_) //============================================================================== template -FCL_EXPORT void DistanceResult::update( S distance, const CollisionGeometry* o1_, @@ -82,7 +80,6 @@ void DistanceResult::update( //============================================================================== template -FCL_EXPORT void DistanceResult::update( S distance, const CollisionGeometry* o1_, @@ -106,7 +103,6 @@ void DistanceResult::update( //============================================================================== template -FCL_EXPORT void DistanceResult::update(const DistanceResult& other_result) { if(min_distance > other_result.min_distance) @@ -123,7 +119,6 @@ void DistanceResult::update(const DistanceResult& other_result) //============================================================================== template -FCL_EXPORT void DistanceResult::clear() { min_distance = std::numeric_limits::max(); diff --git a/include/fcl/narrowphase/distance_result.h b/include/fcl/narrowphase/distance_result.h index 045d2c80d..c555858f0 100644 --- a/include/fcl/narrowphase/distance_result.h +++ b/include/fcl/narrowphase/distance_result.h @@ -48,7 +48,7 @@ class CollisionGeometry; /// @brief distance result template -struct FCL_EXPORT DistanceResult +struct DistanceResult { public: @@ -88,7 +88,7 @@ struct FCL_EXPORT DistanceResult /// @brief invalid contact primitive information static const int NONE = -1; - + DistanceResult(S min_distance_ = std::numeric_limits::max()); /// @brief add distance information into the result diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index dc9271654..a80060abf 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -41,6 +41,6 @@ namespace fcl { template -class SSaPCollisionManager; +class FCL_INSTANTIATION_DEF_API SSaPCollisionManager; } // namespace diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index cee81176a..60fe06e06 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -41,6 +41,6 @@ namespace fcl { template -class SaPCollisionManager; +class FCL_INSTANTIATION_DEF_API SaPCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index a8f533988..3ff81662d 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -41,6 +41,6 @@ namespace fcl { template -class NaiveCollisionManager; +class FCL_INSTANTIATION_DEF_API NaiveCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp index 3cecc24f7..8f3b24335 100644 --- a/src/broadphase/broadphase_collision_manager.cpp +++ b/src/broadphase/broadphase_collision_manager.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -41,6 +41,6 @@ namespace fcl { template -class BroadPhaseCollisionManager; +class FCL_INSTANTIATION_DEF_API BroadPhaseCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_continuous_collision_manager.cpp b/src/broadphase/broadphase_continuous_collision_manager.cpp index d06760a53..7084d769e 100644 --- a/src/broadphase/broadphase_continuous_collision_manager.cpp +++ b/src/broadphase/broadphase_continuous_collision_manager.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -41,6 +41,6 @@ namespace fcl { template -class BroadPhaseContinuousCollisionManager; +class FCL_INSTANTIATION_DEF_API BroadPhaseContinuousCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 1da6ca3ca..e43bcdb9d 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -41,6 +41,6 @@ namespace fcl { template -class DynamicAABBTreeCollisionManager; +class FCL_INSTANTIATION_DEF_API DynamicAABBTreeCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 515feda97..375c78b1d 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -41,6 +41,6 @@ namespace fcl { template -class DynamicAABBTreeCollisionManager_Array; +class FCL_INSTANTIATION_DEF_API DynamicAABBTreeCollisionManager_Array; } // namespace fcl diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index 3567fb175..10c0cf368 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -41,6 +41,6 @@ namespace fcl { template -class IntervalTreeCollisionManager; +class FCL_INSTANTIATION_DEF_API IntervalTreeCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_spatialhash.cpp b/src/broadphase/broadphase_spatialhash.cpp index 3a9d90eee..0592102b4 100644 --- a/src/broadphase/broadphase_spatialhash.cpp +++ b/src/broadphase/broadphase_spatialhash.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -41,7 +41,7 @@ namespace fcl { template -class SpatialHashingCollisionManager< +class FCL_INSTANTIATION_DEF_API SpatialHashingCollisionManager< double, detail::SimpleHashTable< AABB, CollisionObject*, detail::SpatialHash>>; diff --git a/src/broadphase/detail/interval_tree.cpp b/src/broadphase/detail/interval_tree.cpp index 28419ff2b..1cc1a5346 100644 --- a/src/broadphase/detail/interval_tree.cpp +++ b/src/broadphase/detail/interval_tree.cpp @@ -44,10 +44,10 @@ namespace detail { template -struct it_recursion_node; +struct FCL_INSTANTIATION_DEF_API it_recursion_node; template -class IntervalTree; +class FCL_INSTANTIATION_DEF_API IntervalTree; } // namespace detail } // namespace fcl diff --git a/src/broadphase/detail/interval_tree_node.cpp b/src/broadphase/detail/interval_tree_node.cpp index b3779b78c..1fe47a325 100644 --- a/src/broadphase/detail/interval_tree_node.cpp +++ b/src/broadphase/detail/interval_tree_node.cpp @@ -44,7 +44,7 @@ namespace detail { template -class IntervalTreeNode; +class FCL_INSTANTIATION_DEF_API IntervalTreeNode; } // namespace detail } // namespace fcl diff --git a/src/broadphase/detail/morton.cpp b/src/broadphase/detail/morton.cpp index fa0419f72..035644ccd 100644 --- a/src/broadphase/detail/morton.cpp +++ b/src/broadphase/detail/morton.cpp @@ -47,6 +47,7 @@ namespace detail //============================================================================== template +FCL_INSTANTIATION_DEF_API uint32 quantize(double x, uint32 n); //============================================================================== @@ -86,11 +87,11 @@ uint64 morton_code60(uint32 x, uint32 y, uint32 z) //============================================================================== template -struct morton_functor; +struct FCL_INSTANTIATION_DEF_API morton_functor; //============================================================================== template -struct morton_functor; +struct FCL_INSTANTIATION_DEF_API morton_functor; } // namespace detail /// @endcond diff --git a/src/broadphase/detail/simple_interval.cpp b/src/broadphase/detail/simple_interval.cpp index 0271d9bde..01faeed0f 100644 --- a/src/broadphase/detail/simple_interval.cpp +++ b/src/broadphase/detail/simple_interval.cpp @@ -44,7 +44,7 @@ namespace detail { template -struct SimpleInterval; +struct FCL_INSTANTIATION_DEF_API SimpleInterval; } // namespace detail } // namespace fcl diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp index 01a8e25aa..19450b399 100644 --- a/src/broadphase/detail/spatial_hash.cpp +++ b/src/broadphase/detail/spatial_hash.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -44,7 +44,7 @@ namespace detail { template -struct SpatialHash; +struct FCL_INSTANTIATION_DEF_API SpatialHash; } // namespace detail } // namespace fcl diff --git a/src/common/detail/profiler.cpp b/src/common/detail/profiler.cpp index 6d78ee0d5..47d4ffcdb 100644 --- a/src/common/detail/profiler.cpp +++ b/src/common/detail/profiler.cpp @@ -238,14 +238,14 @@ bool Profiler::Running() } //============================================================================== -struct FCL_EXPORT dataIntVal +struct dataIntVal { std::string name; unsigned long int value; }; //============================================================================== -struct FCL_EXPORT SortIntByValue +struct SortIntByValue { bool operator()(const dataIntVal &a, const dataIntVal &b) const { @@ -254,14 +254,14 @@ struct FCL_EXPORT SortIntByValue }; //============================================================================== -struct FCL_EXPORT dataDoubleVal +struct dataDoubleVal { std::string name; double value; }; //============================================================================== -struct FCL_EXPORT SortDoubleByValue +struct SortDoubleByValue { bool operator()(const dataDoubleVal &a, const dataDoubleVal &b) const { diff --git a/src/geometry/bvh/BVH_utility.cpp b/src/geometry/bvh/BVH_utility.cpp index bfcc8e882..de6e1f013 100644 --- a/src/geometry/bvh/BVH_utility.cpp +++ b/src/geometry/bvh/BVH_utility.cpp @@ -42,11 +42,13 @@ namespace fcl //============================================================================== template +FCL_INSTANTIATION_DEF_API void BVHExpand( BVHModel>& model, const Variance3* ucs, double r); //============================================================================== template +FCL_INSTANTIATION_DEF_API void BVHExpand( BVHModel>& model, const Variance3* ucs, double r); diff --git a/src/geometry/collision_geometry.cpp b/src/geometry/collision_geometry.cpp index a898ecde9..83d96e47a 100644 --- a/src/geometry/collision_geometry.cpp +++ b/src/geometry/collision_geometry.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class CollisionGeometry; +class FCL_INSTANTIATION_DEF_API CollisionGeometry; } // namespace fcl diff --git a/src/geometry/octree/octree.cpp b/src/geometry/octree/octree.cpp index 1e392e9d2..f94622fae 100644 --- a/src/geometry/octree/octree.cpp +++ b/src/geometry/octree/octree.cpp @@ -46,12 +46,13 @@ namespace fcl //============================================================================== template -class OcTree; +class FCL_INSTANTIATION_DEF_API OcTree; //============================================================================== template +FCL_INSTANTIATION_DEF_API void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv); } // namespace fcl -#endif \ No newline at end of file +#endif diff --git a/src/geometry/shape/box.cpp b/src/geometry/shape/box.cpp index 9c96f1c86..1d1f4e17b 100644 --- a/src/geometry/shape/box.cpp +++ b/src/geometry/shape/box.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class Box; +class FCL_INSTANTIATION_DEF_API Box; } // namespace fcl diff --git a/src/geometry/shape/capsule.cpp b/src/geometry/shape/capsule.cpp index db46b668b..594fe0a65 100644 --- a/src/geometry/shape/capsule.cpp +++ b/src/geometry/shape/capsule.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class Capsule; +class FCL_INSTANTIATION_DEF_API Capsule; } // namespace fcl diff --git a/src/geometry/shape/cone.cpp b/src/geometry/shape/cone.cpp index f201ebbd0..0bb75d186 100644 --- a/src/geometry/shape/cone.cpp +++ b/src/geometry/shape/cone.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class Cone; +class FCL_INSTANTIATION_DEF_API Cone; } // namespace fcl diff --git a/src/geometry/shape/convex.cpp b/src/geometry/shape/convex.cpp index 424292371..7b4fc5b86 100644 --- a/src/geometry/shape/convex.cpp +++ b/src/geometry/shape/convex.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class Convex; +class FCL_INSTANTIATION_DEF_API Convex; } // namespace fcl diff --git a/src/geometry/shape/cylinder.cpp b/src/geometry/shape/cylinder.cpp index 99933f9ac..be61aee4d 100644 --- a/src/geometry/shape/cylinder.cpp +++ b/src/geometry/shape/cylinder.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class Cylinder; +class FCL_INSTANTIATION_DEF_API Cylinder; } // namespace fcl diff --git a/src/geometry/shape/ellipsoid.cpp b/src/geometry/shape/ellipsoid.cpp index d30baea30..9e26db205 100644 --- a/src/geometry/shape/ellipsoid.cpp +++ b/src/geometry/shape/ellipsoid.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class Ellipsoid; +class FCL_INSTANTIATION_DEF_API Ellipsoid; } // namespace fcl diff --git a/src/geometry/shape/halfspace.cpp b/src/geometry/shape/halfspace.cpp index f01e946a4..e887371c1 100644 --- a/src/geometry/shape/halfspace.cpp +++ b/src/geometry/shape/halfspace.cpp @@ -41,9 +41,10 @@ namespace fcl { template -class Halfspace; +class FCL_INSTANTIATION_DEF_API Halfspace; template +FCL_INSTANTIATION_DEF_API Halfspace transform(const Halfspace& a, const Transform3& tf); } // namespace fcl diff --git a/src/geometry/shape/plane.cpp b/src/geometry/shape/plane.cpp index e61fa63b7..fe7b5912c 100644 --- a/src/geometry/shape/plane.cpp +++ b/src/geometry/shape/plane.cpp @@ -42,10 +42,11 @@ namespace fcl //============================================================================== template -class Plane; +class FCL_INSTANTIATION_DEF_API Plane; //============================================================================== template +FCL_INSTANTIATION_DEF_API Plane transform(const Plane& a, const Transform3& tf); } // namespace fcl diff --git a/src/geometry/shape/shape_base.cpp b/src/geometry/shape/shape_base.cpp index 179d73ebb..98d65e554 100644 --- a/src/geometry/shape/shape_base.cpp +++ b/src/geometry/shape/shape_base.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class ShapeBase; +class FCL_INSTANTIATION_DEF_API ShapeBase; } // namespace fcl diff --git a/src/geometry/shape/sphere.cpp b/src/geometry/shape/sphere.cpp index b8a857808..7d4a9429f 100644 --- a/src/geometry/shape/sphere.cpp +++ b/src/geometry/shape/sphere.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class Sphere; +class FCL_INSTANTIATION_DEF_API Sphere; } // namespace fcl diff --git a/src/geometry/shape/triangle_p.cpp b/src/geometry/shape/triangle_p.cpp index 86546be31..a375fb879 100644 --- a/src/geometry/shape/triangle_p.cpp +++ b/src/geometry/shape/triangle_p.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class TriangleP; +class FCL_INSTANTIATION_DEF_API TriangleP; } // namespace fcl diff --git a/src/geometry/shape/utility.cpp b/src/geometry/shape/utility.cpp index a64ee2511..e3737b5df 100644 --- a/src/geometry/shape/utility.cpp +++ b/src/geometry/shape/utility.cpp @@ -41,66 +41,82 @@ namespace fcl { //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const AABB& bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const OBB& bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const OBBRSS& bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const kIOS& bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const RSS& bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const AABB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const OBB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const OBBRSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const kIOS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const RSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== @@ -109,119 +125,119 @@ namespace detail { //============================================================================== template -struct ComputeBVImpl, Box>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Box>; //============================================================================== template -struct ComputeBVImpl, Box>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Box>; //============================================================================== template -struct ComputeBVImpl, Capsule>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Capsule>; //============================================================================== template -struct ComputeBVImpl, Capsule>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Capsule>; //============================================================================== template -struct ComputeBVImpl, Cone>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Cone>; //============================================================================== template -struct ComputeBVImpl, Cone>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Cone>; //============================================================================== template -struct ComputeBVImpl, Cylinder>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Cylinder>; //============================================================================== template -struct ComputeBVImpl, Cylinder>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Cylinder>; //============================================================================== template -struct ComputeBVImpl, Ellipsoid>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Ellipsoid>; //============================================================================== template -struct ComputeBVImpl, Ellipsoid>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Ellipsoid>; //============================================================================== template -struct ComputeBVImpl, Halfspace>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; //============================================================================== template -struct ComputeBVImpl, Halfspace>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; //============================================================================== template -struct ComputeBVImpl, Halfspace>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; //============================================================================== template -struct ComputeBVImpl, Halfspace>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; //============================================================================== template -struct ComputeBVImpl, Halfspace>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; //============================================================================== template -struct ComputeBVImpl, Halfspace>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; //============================================================================== template -struct ComputeBVImpl, Halfspace>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; //============================================================================== template -struct ComputeBVImpl, Halfspace>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; //============================================================================== template -struct ComputeBVImpl, Plane>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; //============================================================================== template -struct ComputeBVImpl, Plane>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; //============================================================================== template -struct ComputeBVImpl, Plane>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; //============================================================================== template -struct ComputeBVImpl, Plane>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; //============================================================================== template -struct ComputeBVImpl, Plane>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; //============================================================================== template -struct ComputeBVImpl, Plane>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; //============================================================================== template -struct ComputeBVImpl, Plane>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; //============================================================================== template -struct ComputeBVImpl, Plane>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; //============================================================================== template -struct ComputeBVImpl, Sphere>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Sphere>; //============================================================================== template -struct ComputeBVImpl, Sphere>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Sphere>; //============================================================================== template -struct ComputeBVImpl, TriangleP>; +struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, TriangleP>; //============================================================================== } // namespace detail diff --git a/src/math/bv/AABB.cpp b/src/math/bv/AABB.cpp index a7c382422..9696c758c 100644 --- a/src/math/bv/AABB.cpp +++ b/src/math/bv/AABB.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class AABB; +class FCL_INSTANTIATION_DEF_API AABB; } // namespace fcl diff --git a/src/math/bv/OBB.cpp b/src/math/bv/OBB.cpp index 1cfa1f6db..af100579a 100644 --- a/src/math/bv/OBB.cpp +++ b/src/math/bv/OBB.cpp @@ -42,22 +42,26 @@ namespace fcl //============================================================================== template -class OBB; +class FCL_INSTANTIATION_DEF_API OBB; //============================================================================== template +FCL_INSTANTIATION_DEF_API void computeVertices(const OBB& b, Vector3 vertices[8]); //============================================================================== template +FCL_INSTANTIATION_DEF_API OBB merge_largedist(const OBB& b1, const OBB& b2); //============================================================================== template +FCL_INSTANTIATION_DEF_API OBB merge_smalldist(const OBB& b1, const OBB& b2); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool obbDisjoint( const Matrix3& B, const Vector3& T, @@ -66,6 +70,7 @@ bool obbDisjoint( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool obbDisjoint( const Transform3& tf, const Vector3& a, diff --git a/src/math/bv/OBBRSS.cpp b/src/math/bv/OBBRSS.cpp index 10cb7941a..84278a2f0 100644 --- a/src/math/bv/OBBRSS.cpp +++ b/src/math/bv/OBBRSS.cpp @@ -42,10 +42,11 @@ namespace fcl //============================================================================== template -class OBBRSS; +class FCL_INSTANTIATION_DEF_API OBBRSS; //============================================================================== template +FCL_INSTANTIATION_DEF_API OBBRSS translate(const OBBRSS& bv, const Vector3& t); } // namespace fcl diff --git a/src/math/bv/RSS.cpp b/src/math/bv/RSS.cpp index c7db037ca..b2aff7af2 100644 --- a/src/math/bv/RSS.cpp +++ b/src/math/bv/RSS.cpp @@ -42,14 +42,16 @@ namespace fcl //============================================================================== template -class RSS; +class FCL_INSTANTIATION_DEF_API RSS; //============================================================================== template +FCL_INSTANTIATION_DEF_API void clipToRange(double& val, double a, double b); //============================================================================== template +FCL_INSTANTIATION_DEF_API void segCoords( double& t, double& u, @@ -61,6 +63,7 @@ void segCoords( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool inVoronoi( double a, double b, @@ -72,6 +75,7 @@ bool inVoronoi( //============================================================================== template +FCL_INSTANTIATION_DEF_API double rectDistance( const Matrix3& Rab, const Vector3& Tab, @@ -82,6 +86,7 @@ double rectDistance( //============================================================================== template +FCL_INSTANTIATION_DEF_API double rectDistance( const Transform3& tfab, const double a[2], @@ -91,6 +96,7 @@ double rectDistance( //============================================================================== template +FCL_INSTANTIATION_DEF_API RSS translate(const RSS& bv, const Vector3& t); } // namespace fcl diff --git a/src/math/bv/kDOP.cpp b/src/math/bv/kDOP.cpp index 8879a4409..e8901914b 100644 --- a/src/math/bv/kDOP.cpp +++ b/src/math/bv/kDOP.cpp @@ -42,34 +42,39 @@ namespace fcl //============================================================================== template -class KDOP; +class FCL_INSTANTIATION_DEF_API KDOP; //============================================================================== template -class KDOP; +class FCL_INSTANTIATION_DEF_API KDOP; //============================================================================== template -class KDOP; +class FCL_INSTANTIATION_DEF_API KDOP; //============================================================================== template +FCL_INSTANTIATION_DEF_API void minmax(double a, double b, double& minv, double& maxv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void minmax(double p, double& minv, double& maxv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void getDistances(const Vector3& p, double* d); //============================================================================== template +FCL_INSTANTIATION_DEF_API void getDistances(const Vector3& p, double* d); //============================================================================== template +FCL_INSTANTIATION_DEF_API void getDistances(const Vector3& p, double* d); } // namespace fcl diff --git a/src/math/bv/kIOS.cpp b/src/math/bv/kIOS.cpp index 4c17b9224..8c62ab7c3 100644 --- a/src/math/bv/kIOS.cpp +++ b/src/math/bv/kIOS.cpp @@ -39,8 +39,8 @@ namespace fcl { - + template -class kIOS; +class FCL_INSTANTIATION_DEF_API kIOS; } // namespace fcl diff --git a/src/math/bv/utility.cpp b/src/math/bv/utility.cpp index d2e44a9c9..505236598 100644 --- a/src/math/bv/utility.cpp +++ b/src/math/bv/utility.cpp @@ -46,22 +46,27 @@ namespace OBB_fit_functions { //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit1(const Vector3d* const ps, OBB& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit2(const Vector3d* const ps, OBB& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit3(const Vector3d* const ps, OBB& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit6(const Vector3d* const ps, OBB& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fitn(const Vector3d* const ps, int n, OBB& bv); //============================================================================== @@ -74,22 +79,27 @@ namespace RSS_fit_functions { //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit1(const Vector3d* const ps, RSS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit2(const Vector3d* const ps, RSS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit3(const Vector3d* const ps, RSS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit6(const Vector3d* const ps, RSS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fitn(const Vector3d* const ps, int n, RSS& bv); //============================================================================== @@ -102,18 +112,22 @@ namespace kIOS_fit_functions { //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit1(const Vector3d* const ps, kIOS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit2(const Vector3d* const ps, kIOS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit3(const Vector3d* const ps, kIOS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fitn(const Vector3d* const ps, int n, kIOS& bv); //============================================================================== @@ -126,18 +140,22 @@ namespace OBBRSS_fit_functions { //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit1(const Vector3d* const ps, OBBRSS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit2(const Vector3d* const ps, OBBRSS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fit3(const Vector3d* const ps, OBBRSS& bv); //============================================================================== template +FCL_INSTANTIATION_DEF_API void fitn(const Vector3d* const ps, int n, OBBRSS& bv); //============================================================================== @@ -146,55 +164,55 @@ void fitn(const Vector3d* const ps, int n, OBBRSS& bv); //============================================================================== template -struct Fitter>; +struct FCL_INSTANTIATION_DEF_API Fitter>; //============================================================================== template -struct Fitter>; +struct FCL_INSTANTIATION_DEF_API Fitter>; //============================================================================== template -struct Fitter>; +struct FCL_INSTANTIATION_DEF_API Fitter>; //============================================================================== template -struct Fitter>; +struct FCL_INSTANTIATION_DEF_API Fitter>; //============================================================================== template -class ConvertBVImpl, AABB>; +class FCL_INSTANTIATION_DEF_API ConvertBVImpl, AABB>; //============================================================================== template -class ConvertBVImpl, OBB>; +class FCL_INSTANTIATION_DEF_API ConvertBVImpl, OBB>; //============================================================================== template -class ConvertBVImpl, OBB>; +class FCL_INSTANTIATION_DEF_API ConvertBVImpl, OBB>; //============================================================================== template -class ConvertBVImpl, OBB>; +class FCL_INSTANTIATION_DEF_API ConvertBVImpl, OBB>; //============================================================================== template -class ConvertBVImpl, OBB>; +class FCL_INSTANTIATION_DEF_API ConvertBVImpl, OBB>; //============================================================================== template -class ConvertBVImpl, RSS>; +class FCL_INSTANTIATION_DEF_API ConvertBVImpl, RSS>; //============================================================================== template -class ConvertBVImpl, RSS>; +class FCL_INSTANTIATION_DEF_API ConvertBVImpl, RSS>; //============================================================================== template -class ConvertBVImpl, RSS>; +class FCL_INSTANTIATION_DEF_API ConvertBVImpl, RSS>; //============================================================================== template -class ConvertBVImpl, RSS>; +class FCL_INSTANTIATION_DEF_API ConvertBVImpl, RSS>; } // namespace detail } // namespace fcl diff --git a/src/math/detail/polysolver.cpp b/src/math/detail/polysolver.cpp index 3aedfa003..222063407 100644 --- a/src/math/detail/polysolver.cpp +++ b/src/math/detail/polysolver.cpp @@ -1,49 +1,49 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#include "fcl/math/detail/polysolver-inl.h" - -namespace fcl -{ - -namespace detail { - -template -class PolySolver; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#include "fcl/math/detail/polysolver-inl.h" + +namespace fcl +{ + +namespace detail { + +template +class FCL_INSTANTIATION_DEF_API PolySolver; + +} // namespace detail +} // namespace fcl diff --git a/src/math/detail/project.cpp b/src/math/detail/project.cpp index e5bbe9472..b26fecddf 100644 --- a/src/math/detail/project.cpp +++ b/src/math/detail/project.cpp @@ -1,50 +1,50 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#include "fcl/math/detail/project-inl.h" - -namespace fcl -{ - -namespace detail -{ - -template -class Project; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#include "fcl/math/detail/project-inl.h" + +namespace fcl +{ + +namespace detail +{ + +template +class FCL_INSTANTIATION_DEF_API Project; + +} // namespace detail +} // namespace fcl diff --git a/src/math/geometry.cpp b/src/math/geometry.cpp index b40110f69..e14c52104 100644 --- a/src/math/geometry.cpp +++ b/src/math/geometry.cpp @@ -41,41 +41,50 @@ namespace fcl { //============================================================================== template +FCL_INSTANTIATION_DEF_API void normalize(Vector3d& v, bool* signal); //============================================================================== template +FCL_INSTANTIATION_DEF_API void hat(Matrix3d& mat, const Vector3d& vec); //============================================================================== template +FCL_INSTANTIATION_DEF_API void eigen(const Matrix3d& m, Vector3d& dout, Matrix3d& vout); //============================================================================== template +FCL_INSTANTIATION_DEF_API void eigen_old(const Matrix3d& m, Vector3d& dout, Matrix3d& vout); //============================================================================== template +FCL_INSTANTIATION_DEF_API void axisFromEigen( const Matrix3d& eigenV, const Vector3d& eigenS, Matrix3d& axis); //============================================================================== template +FCL_INSTANTIATION_DEF_API void axisFromEigen(const Matrix3d& eigenV, const Vector3d& eigenS, Transform3d& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void generateCoordinateSystem(Matrix3d& axis); //============================================================================== template +FCL_INSTANTIATION_DEF_API void generateCoordinateSystem(Transform3d& tf); //============================================================================== template +FCL_INSTANTIATION_DEF_API void getRadiusAndOriginAndRectangleSize( const Vector3d* const ps, const Vector3d* const ps2, @@ -89,6 +98,7 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== template +FCL_INSTANTIATION_DEF_API void getRadiusAndOriginAndRectangleSize( const Vector3d* const ps, const Vector3d* const ps2, @@ -101,6 +111,7 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== template +FCL_INSTANTIATION_DEF_API void circumCircleComputation( const Vector3d& a, const Vector3d& b, @@ -110,6 +121,7 @@ void circumCircleComputation( //============================================================================== template +FCL_INSTANTIATION_DEF_API double maximumDistance( const Vector3d* const ps, const Vector3d* const ps2, @@ -120,6 +132,7 @@ double maximumDistance( //============================================================================== template +FCL_INSTANTIATION_DEF_API void getExtentAndCenter( const Vector3d* const ps, const Vector3d* const ps2, @@ -132,6 +145,7 @@ void getExtentAndCenter( //============================================================================== template +FCL_INSTANTIATION_DEF_API void getCovariance( const Vector3d* const ps, const Vector3d* const ps2, @@ -145,6 +159,7 @@ namespace detail { //============================================================================== template +FCL_INSTANTIATION_DEF_API double maximumDistance_mesh( const Vector3d* const ps, const Vector3d* const ps2, @@ -155,6 +170,7 @@ double maximumDistance_mesh( //============================================================================== template +FCL_INSTANTIATION_DEF_API double maximumDistance_pointcloud( const Vector3d* const ps, const Vector3d* const ps2, @@ -164,6 +180,7 @@ double maximumDistance_pointcloud( //============================================================================== template +FCL_INSTANTIATION_DEF_API void getExtentAndCenter_pointcloud( const Vector3d* const ps, const Vector3d* const ps2, @@ -175,6 +192,7 @@ void getExtentAndCenter_pointcloud( //============================================================================== template +FCL_INSTANTIATION_DEF_API void getExtentAndCenter_mesh( const Vector3d* const ps, const Vector3d* const ps2, diff --git a/src/math/motion/interp_motion.cpp b/src/math/motion/interp_motion.cpp index aa5e592c9..debade816 100644 --- a/src/math/motion/interp_motion.cpp +++ b/src/math/motion/interp_motion.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class InterpMotion; +class FCL_INSTANTIATION_DEF_API InterpMotion; } // namespace fcl diff --git a/src/math/motion/motion_base.cpp b/src/math/motion/motion_base.cpp index 981cc8198..1c21cdfd0 100644 --- a/src/math/motion/motion_base.cpp +++ b/src/math/motion/motion_base.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class MotionBase; +class FCL_INSTANTIATION_DEF_API MotionBase; } // namespace fcl diff --git a/src/math/motion/screw_motion.cpp b/src/math/motion/screw_motion.cpp index 65d1066ea..9958e8f7f 100644 --- a/src/math/motion/screw_motion.cpp +++ b/src/math/motion/screw_motion.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class ScrewMotion; +class FCL_INSTANTIATION_DEF_API ScrewMotion; } // namespace fcl diff --git a/src/math/motion/spline_motion.cpp b/src/math/motion/spline_motion.cpp index 16601e247..983d82516 100644 --- a/src/math/motion/spline_motion.cpp +++ b/src/math/motion/spline_motion.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class SplineMotion; +class FCL_INSTANTIATION_DEF_API SplineMotion; } // namespace fcl diff --git a/src/math/motion/taylor_model/interval.cpp b/src/math/motion/taylor_model/interval.cpp index 236ec3d46..b7c7932de 100644 --- a/src/math/motion/taylor_model/interval.cpp +++ b/src/math/motion/taylor_model/interval.cpp @@ -43,14 +43,16 @@ namespace fcl //============================================================================== template -struct Interval; +struct FCL_INSTANTIATION_DEF_API Interval; //============================================================================== template +FCL_INSTANTIATION_DEF_API Interval bound(const Interval& i, double v); //============================================================================== template +FCL_INSTANTIATION_DEF_API Interval bound(const Interval& i, const Interval& other); } // namespace fcl diff --git a/src/math/motion/taylor_model/interval_matrix.cpp b/src/math/motion/taylor_model/interval_matrix.cpp index 1bb0f9280..e2a2d18fe 100644 --- a/src/math/motion/taylor_model/interval_matrix.cpp +++ b/src/math/motion/taylor_model/interval_matrix.cpp @@ -42,10 +42,11 @@ namespace fcl //============================================================================== template -struct IMatrix3; +struct FCL_INSTANTIATION_DEF_API IMatrix3; //============================================================================== template +FCL_INSTANTIATION_DEF_API IMatrix3 rotationConstrain(const IMatrix3& m); } // namespace fcl diff --git a/src/math/motion/taylor_model/interval_vector.cpp b/src/math/motion/taylor_model/interval_vector.cpp index 9e216a637..d841c95d5 100644 --- a/src/math/motion/taylor_model/interval_vector.cpp +++ b/src/math/motion/taylor_model/interval_vector.cpp @@ -42,14 +42,16 @@ namespace fcl //============================================================================== template -struct IVector3; +struct FCL_INSTANTIATION_DEF_API IVector3; //============================================================================== template +FCL_INSTANTIATION_DEF_API IVector3 bound(const IVector3& i, const Vector3& v); //============================================================================== template +FCL_INSTANTIATION_DEF_API IVector3 bound(const IVector3& i, const IVector3& v); } // namespace fcl diff --git a/src/math/motion/taylor_model/taylor_matrix.cpp b/src/math/motion/taylor_model/taylor_matrix.cpp index baa926fd8..d9e0db9c7 100644 --- a/src/math/motion/taylor_model/taylor_matrix.cpp +++ b/src/math/motion/taylor_model/taylor_matrix.cpp @@ -42,34 +42,41 @@ namespace fcl //============================================================================== template -class TMatrix3; +class FCL_INSTANTIATION_DEF_API TMatrix3; //============================================================================== template +FCL_INSTANTIATION_DEF_API TMatrix3 rotationConstrain(const TMatrix3& m); //============================================================================== template +FCL_INSTANTIATION_DEF_API TMatrix3 operator * (const Matrix3& m, const TaylorModel& a); //============================================================================== template +FCL_INSTANTIATION_DEF_API TMatrix3 operator * (const TaylorModel& a, const Matrix3& m); //============================================================================== template +FCL_INSTANTIATION_DEF_API TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m); //============================================================================== template +FCL_INSTANTIATION_DEF_API TMatrix3 operator * (double d, const TMatrix3& m); //============================================================================== template +FCL_INSTANTIATION_DEF_API TMatrix3 operator + (const Matrix3& m1, const TMatrix3& m2); //============================================================================== template +FCL_INSTANTIATION_DEF_API TMatrix3 operator - (const Matrix3& m1, const TMatrix3& m2); } // namespace fcl diff --git a/src/math/motion/taylor_model/taylor_model.cpp b/src/math/motion/taylor_model/taylor_model.cpp index 58a98d73e..fb1a4caf9 100644 --- a/src/math/motion/taylor_model/taylor_model.cpp +++ b/src/math/motion/taylor_model/taylor_model.cpp @@ -45,30 +45,36 @@ namespace fcl //============================================================================== template -class TaylorModel; +class FCL_INSTANTIATION_DEF_API TaylorModel; //============================================================================== template +FCL_INSTANTIATION_DEF_API TaylorModel operator * (double d, const TaylorModel& a); //============================================================================== template +FCL_INSTANTIATION_DEF_API TaylorModel operator + (double d, const TaylorModel& a); //============================================================================== template +FCL_INSTANTIATION_DEF_API TaylorModel operator - (double d, const TaylorModel& a); //============================================================================== template +FCL_INSTANTIATION_DEF_API void generateTaylorModelForCosFunc(TaylorModel& tm, double w, double q0); //============================================================================== template +FCL_INSTANTIATION_DEF_API void generateTaylorModelForSinFunc(TaylorModel& tm, double w, double q0); //============================================================================== template +FCL_INSTANTIATION_DEF_API void generateTaylorModelForLinearFunc(TaylorModel& tm, double p, double v); } // namespace fcl diff --git a/src/math/motion/taylor_model/taylor_vector.cpp b/src/math/motion/taylor_model/taylor_vector.cpp index 81acc6efb..bc22223dc 100644 --- a/src/math/motion/taylor_model/taylor_vector.cpp +++ b/src/math/motion/taylor_model/taylor_vector.cpp @@ -42,22 +42,26 @@ namespace fcl //============================================================================== template -class TVector3; +class FCL_INSTANTIATION_DEF_API TVector3; //============================================================================== template +FCL_INSTANTIATION_DEF_API void generateTVector3ForLinearFunc(TVector3& v, const Vector3& position, const Vector3& velocity); //============================================================================== template +FCL_INSTANTIATION_DEF_API TVector3 operator * (const Vector3& v, const TaylorModel& a); //============================================================================== template +FCL_INSTANTIATION_DEF_API TVector3 operator + (const Vector3& v1, const TVector3& v2); //============================================================================== template +FCL_INSTANTIATION_DEF_API TVector3 operator - (const Vector3& v1, const TVector3& v2); } // namespace fcl diff --git a/src/math/motion/taylor_model/time_interval.cpp b/src/math/motion/taylor_model/time_interval.cpp index 2587db49a..262afe58e 100644 --- a/src/math/motion/taylor_model/time_interval.cpp +++ b/src/math/motion/taylor_model/time_interval.cpp @@ -41,6 +41,6 @@ namespace fcl { template -struct TimeInterval; +struct FCL_INSTANTIATION_DEF_API TimeInterval; } // namespace fcl diff --git a/src/math/motion/translation_motion.cpp b/src/math/motion/translation_motion.cpp index abfbb4165..26e1ba7cb 100644 --- a/src/math/motion/translation_motion.cpp +++ b/src/math/motion/translation_motion.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class TranslationMotion; +class FCL_INSTANTIATION_DEF_API TranslationMotion; } // namespace fcl diff --git a/src/math/motion/triangle_motion_bound_visitor.cpp b/src/math/motion/triangle_motion_bound_visitor.cpp index 1fd1f2b01..f7e237a25 100644 --- a/src/math/motion/triangle_motion_bound_visitor.cpp +++ b/src/math/motion/triangle_motion_bound_visitor.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class TriangleMotionBoundVisitor; +class FCL_INSTANTIATION_DEF_API TriangleMotionBoundVisitor; } // namespace fcl diff --git a/src/math/rng.cpp b/src/math/rng.cpp index c997dab12..34b7864cd 100644 --- a/src/math/rng.cpp +++ b/src/math/rng.cpp @@ -42,6 +42,6 @@ namespace fcl //============================================================================== template -class RNG; +class FCL_INSTANTIATION_DEF_API RNG; } // namespace fcl diff --git a/src/math/sampler/sampler_base.cpp b/src/math/sampler/sampler_base.cpp index 593cbdc9c..5291007f8 100644 --- a/src/math/sampler/sampler_base.cpp +++ b/src/math/sampler/sampler_base.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class SamplerBase; +class FCL_INSTANTIATION_DEF_API SamplerBase; } // namespace fcl diff --git a/src/math/sampler/sampler_se2.cpp b/src/math/sampler/sampler_se2.cpp index 11495c1f9..104b255d7 100644 --- a/src/math/sampler/sampler_se2.cpp +++ b/src/math/sampler/sampler_se2.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class SamplerSE2; +class FCL_INSTANTIATION_DEF_API SamplerSE2; } // namespace fcl diff --git a/src/math/sampler/sampler_se2_disk.cpp b/src/math/sampler/sampler_se2_disk.cpp index d70aab9bb..ba0cad1bf 100644 --- a/src/math/sampler/sampler_se2_disk.cpp +++ b/src/math/sampler/sampler_se2_disk.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class SamplerSE2_disk; +class FCL_INSTANTIATION_DEF_API SamplerSE2_disk; } // namespace fcl diff --git a/src/math/sampler/sampler_se3_euler.cpp b/src/math/sampler/sampler_se3_euler.cpp index 3f97745e1..fe5697abf 100644 --- a/src/math/sampler/sampler_se3_euler.cpp +++ b/src/math/sampler/sampler_se3_euler.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class SamplerSE3Euler; +class FCL_INSTANTIATION_DEF_API SamplerSE3Euler; } // namespace fcl diff --git a/src/math/sampler/sampler_se3_euler_ball.cpp b/src/math/sampler/sampler_se3_euler_ball.cpp index 3570439c3..22257b8b9 100644 --- a/src/math/sampler/sampler_se3_euler_ball.cpp +++ b/src/math/sampler/sampler_se3_euler_ball.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class SamplerSE3Euler_ball; +class FCL_INSTANTIATION_DEF_API SamplerSE3Euler_ball; } // namespace fcl diff --git a/src/math/sampler/sampler_se3_quat.cpp b/src/math/sampler/sampler_se3_quat.cpp index 0d0999e7c..966211bf3 100644 --- a/src/math/sampler/sampler_se3_quat.cpp +++ b/src/math/sampler/sampler_se3_quat.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class SamplerSE3Quat; +class FCL_INSTANTIATION_DEF_API SamplerSE3Quat; } // namespace fcl diff --git a/src/math/sampler/sampler_se3_quat_ball.cpp b/src/math/sampler/sampler_se3_quat_ball.cpp index 0db789a15..78463929e 100644 --- a/src/math/sampler/sampler_se3_quat_ball.cpp +++ b/src/math/sampler/sampler_se3_quat_ball.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class SamplerSE3Quat_ball; +class FCL_INSTANTIATION_DEF_API SamplerSE3Quat_ball; } // namespace fcl diff --git a/src/math/variance3.cpp b/src/math/variance3.cpp index 91255f896..ce08cbc91 100644 --- a/src/math/variance3.cpp +++ b/src/math/variance3.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class Variance3; +class FCL_INSTANTIATION_DEF_API Variance3; } // namespace fcl diff --git a/src/narrowphase/collision.cpp b/src/narrowphase/collision.cpp index ad5186158..70067835f 100644 --- a/src/narrowphase/collision.cpp +++ b/src/narrowphase/collision.cpp @@ -42,6 +42,7 @@ namespace fcl //============================================================================== template +FCL_INSTANTIATION_DEF_API std::size_t collide( const CollisionObject* o1, const CollisionObject* o2, @@ -50,6 +51,7 @@ std::size_t collide( //============================================================================== template +FCL_INSTANTIATION_DEF_API std::size_t collide( const CollisionGeometry* o1, const Transform3& tf1, diff --git a/src/narrowphase/collision_object.cpp b/src/narrowphase/collision_object.cpp index 6362db1f6..97d4e53dc 100644 --- a/src/narrowphase/collision_object.cpp +++ b/src/narrowphase/collision_object.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class CollisionObject; +class FCL_INSTANTIATION_DEF_API CollisionObject; } // namespace fcl diff --git a/src/narrowphase/collision_request.cpp b/src/narrowphase/collision_request.cpp index d47be1ea1..372d8abe4 100644 --- a/src/narrowphase/collision_request.cpp +++ b/src/narrowphase/collision_request.cpp @@ -41,6 +41,6 @@ namespace fcl { template -struct CollisionRequest; +struct FCL_INSTANTIATION_DEF_API CollisionRequest; } // namespace fcl diff --git a/src/narrowphase/collision_result.cpp b/src/narrowphase/collision_result.cpp index 047193b4c..02403cb40 100644 --- a/src/narrowphase/collision_result.cpp +++ b/src/narrowphase/collision_result.cpp @@ -41,6 +41,6 @@ namespace fcl { template -struct CollisionResult; +struct FCL_INSTANTIATION_DEF_API CollisionResult; } // namespace fcl diff --git a/src/narrowphase/contact.cpp b/src/narrowphase/contact.cpp index 4ca0c5ecb..9e45c61ad 100644 --- a/src/narrowphase/contact.cpp +++ b/src/narrowphase/contact.cpp @@ -41,6 +41,6 @@ namespace fcl { template -struct Contact; +struct FCL_INSTANTIATION_DEF_API Contact; } // namespace fcl diff --git a/src/narrowphase/contact_point.cpp b/src/narrowphase/contact_point.cpp index 415e985e1..0c2253fa4 100644 --- a/src/narrowphase/contact_point.cpp +++ b/src/narrowphase/contact_point.cpp @@ -42,15 +42,17 @@ namespace fcl //============================================================================== template -struct ContactPoint; +struct FCL_INSTANTIATION_DEF_API ContactPoint; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool comparePenDepth( const ContactPoint& _cp1, const ContactPoint& _cp2); //============================================================================== template +FCL_INSTANTIATION_DEF_API void flipNormal(std::vector>& contacts); } // namespace fcl diff --git a/src/narrowphase/continuous_collision.cpp b/src/narrowphase/continuous_collision.cpp index bbbce19a9..c62ebcdea 100644 --- a/src/narrowphase/continuous_collision.cpp +++ b/src/narrowphase/continuous_collision.cpp @@ -42,6 +42,7 @@ namespace fcl //============================================================================== template +FCL_INSTANTIATION_DEF_API double continuousCollide( const CollisionGeometry* o1, const MotionBase* motion1, @@ -52,6 +53,7 @@ double continuousCollide( //============================================================================== template +FCL_INSTANTIATION_DEF_API double continuousCollide( const CollisionGeometry* o1, const Transform3& tf1_beg, @@ -64,6 +66,7 @@ double continuousCollide( //============================================================================== template +FCL_INSTANTIATION_DEF_API double continuousCollide( const CollisionObject* o1, const Transform3& tf1_end, @@ -74,6 +77,7 @@ double continuousCollide( //============================================================================== template +FCL_INSTANTIATION_DEF_API double collide( const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, diff --git a/src/narrowphase/continuous_collision_object.cpp b/src/narrowphase/continuous_collision_object.cpp index 93b767204..8719cb900 100644 --- a/src/narrowphase/continuous_collision_object.cpp +++ b/src/narrowphase/continuous_collision_object.cpp @@ -41,6 +41,6 @@ namespace fcl { template -class ContinuousCollisionObject; +class FCL_INSTANTIATION_DEF_API ContinuousCollisionObject; } // namespace fcl diff --git a/src/narrowphase/continuous_collision_request.cpp b/src/narrowphase/continuous_collision_request.cpp index 46dc3ea0e..50042dc15 100644 --- a/src/narrowphase/continuous_collision_request.cpp +++ b/src/narrowphase/continuous_collision_request.cpp @@ -41,6 +41,6 @@ namespace fcl { template -struct ContinuousCollisionRequest; +struct FCL_INSTANTIATION_DEF_API ContinuousCollisionRequest; } // namespace fcl diff --git a/src/narrowphase/continuous_collision_result.cpp b/src/narrowphase/continuous_collision_result.cpp index 3d9c559f2..e6a203191 100644 --- a/src/narrowphase/continuous_collision_result.cpp +++ b/src/narrowphase/continuous_collision_result.cpp @@ -41,6 +41,6 @@ namespace fcl { template -struct ContinuousCollisionResult; +struct FCL_INSTANTIATION_DEF_API ContinuousCollisionResult; } // namespace fcl diff --git a/src/narrowphase/cost_source.cpp b/src/narrowphase/cost_source.cpp index 9c606381a..482b93896 100644 --- a/src/narrowphase/cost_source.cpp +++ b/src/narrowphase/cost_source.cpp @@ -41,6 +41,6 @@ namespace fcl { template -struct CostSource; +struct FCL_INSTANTIATION_DEF_API CostSource; } // namespace fcl diff --git a/src/narrowphase/detail/convexity_based_algorithm/epa.cpp b/src/narrowphase/detail/convexity_based_algorithm/epa.cpp index eae9f1784..136851b3a 100644 --- a/src/narrowphase/detail/convexity_based_algorithm/epa.cpp +++ b/src/narrowphase/detail/convexity_based_algorithm/epa.cpp @@ -44,7 +44,7 @@ namespace detail { template -struct EPA; +struct FCL_INSTANTIATION_DEF_API EPA; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/convexity_based_algorithm/gjk.cpp b/src/narrowphase/detail/convexity_based_algorithm/gjk.cpp index 6b693ebe9..4cb1747f0 100644 --- a/src/narrowphase/detail/convexity_based_algorithm/gjk.cpp +++ b/src/narrowphase/detail/convexity_based_algorithm/gjk.cpp @@ -44,7 +44,7 @@ namespace detail { template -struct GJK; +struct FCL_INSTANTIATION_DEF_API GJK; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/convexity_based_algorithm/gjk_libccd.cpp b/src/narrowphase/detail/convexity_based_algorithm/gjk_libccd.cpp index 7c3714e59..a9592c8e7 100644 --- a/src/narrowphase/detail/convexity_based_algorithm/gjk_libccd.cpp +++ b/src/narrowphase/detail/convexity_based_algorithm/gjk_libccd.cpp @@ -45,39 +45,41 @@ namespace detail //============================================================================== template -class GJKInitializer>; +class FCL_INSTANTIATION_DEF_API GJKInitializer>; //============================================================================== template -class GJKInitializer>; +class FCL_INSTANTIATION_DEF_API GJKInitializer>; //============================================================================== template -class GJKInitializer>; +class FCL_INSTANTIATION_DEF_API GJKInitializer>; //============================================================================== template -class GJKInitializer>; +class FCL_INSTANTIATION_DEF_API GJKInitializer>; //============================================================================== template -class GJKInitializer>; +class FCL_INSTANTIATION_DEF_API GJKInitializer>; //============================================================================== template -class GJKInitializer>; +class FCL_INSTANTIATION_DEF_API GJKInitializer>; //============================================================================== template -class GJKInitializer>; +class FCL_INSTANTIATION_DEF_API GJKInitializer>; //============================================================================== template +FCL_INSTANTIATION_DEF_API void* triCreateGJKObject( const Vector3d& P1, const Vector3d& P2, const Vector3d& P3); //============================================================================== template +FCL_INSTANTIATION_DEF_API void* triCreateGJKObject( const Vector3d& P1, const Vector3d& P2, @@ -86,6 +88,7 @@ void* triCreateGJKObject( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool GJKCollide( void* obj1, ccd_support_fn supp1, @@ -101,6 +104,7 @@ bool GJKCollide( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool GJKDistance( void* obj1, ccd_support_fn supp1, @@ -113,6 +117,7 @@ bool GJKDistance( Vector3d* p2); template +FCL_INSTANTIATION_DEF_API bool GJKSignedDistance( void* obj1, ccd_support_fn supp1, diff --git a/src/narrowphase/detail/convexity_based_algorithm/minkowski_diff.cpp b/src/narrowphase/detail/convexity_based_algorithm/minkowski_diff.cpp index af2d7117b..0f0dbb6cc 100644 --- a/src/narrowphase/detail/convexity_based_algorithm/minkowski_diff.cpp +++ b/src/narrowphase/detail/convexity_based_algorithm/minkowski_diff.cpp @@ -44,7 +44,7 @@ namespace detail { template -struct MinkowskiDiff; +struct FCL_INSTANTIATION_DEF_API MinkowskiDiff; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/gjk_solver_indep.cpp b/src/narrowphase/detail/gjk_solver_indep.cpp index 8c17e3a77..6a881d090 100755 --- a/src/narrowphase/detail/gjk_solver_indep.cpp +++ b/src/narrowphase/detail/gjk_solver_indep.cpp @@ -44,7 +44,7 @@ namespace detail { template -struct GJKSolver_indep; +struct FCL_INSTANTIATION_DEF_API GJKSolver_indep; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/gjk_solver_libccd.cpp b/src/narrowphase/detail/gjk_solver_libccd.cpp index e906a069f..d43cd7571 100755 --- a/src/narrowphase/detail/gjk_solver_libccd.cpp +++ b/src/narrowphase/detail/gjk_solver_libccd.cpp @@ -44,7 +44,7 @@ namespace detail { template -struct GJKSolver_libccd; +struct FCL_INSTANTIATION_DEF_API GJKSolver_libccd; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/box_box.cpp b/src/narrowphase/detail/primitive_shape_algorithm/box_box.cpp index aa2718274..481d8ce75 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/box_box.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/box_box.cpp @@ -45,20 +45,24 @@ namespace detail //============================================================================== template +FCL_INSTANTIATION_DEF_API void lineClosestApproach(const Vector3& pa, const Vector3& ua, const Vector3& pb, const Vector3& ub, double* alpha, double* beta); //============================================================================== template +FCL_INSTANTIATION_DEF_API int intersectRectQuad2(double h[2], double p[8], double ret[16]); //============================================================================== template +FCL_INSTANTIATION_DEF_API void cullPoints2(int n, double p[], int m, int i0, int iret[]); //============================================================================== template +FCL_INSTANTIATION_DEF_API int boxBox2( const Vector3& side1, const Transform3& tf1, @@ -72,6 +76,7 @@ int boxBox2( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool boxBoxIntersect(const Box& s1, const Transform3& tf1, const Box& s2, const Transform3& tf2, std::vector>* contacts_); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.cpp b/src/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.cpp index bf72d5cfa..f481b6dc4 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.cpp @@ -45,18 +45,22 @@ namespace detail //============================================================================== template +FCL_INSTANTIATION_DEF_API double clamp(double n, double min, double max); //============================================================================== -template double closestPtSegmentSegment(const Vector3d& p_FP1, - const Vector3d& p_FQ1, - const Vector3d& p_FP2, - const Vector3d& p_FQ2, double* s, - double* t, Vector3d* p_FC1, - Vector3d* p_FC2); +template +FCL_INSTANTIATION_DEF_API +double closestPtSegmentSegment(const Vector3d& p_FP1, + const Vector3d& p_FQ1, + const Vector3d& p_FP2, + const Vector3d& p_FQ2, double* s, + double* t, Vector3d* p_FC1, + Vector3d* p_FC2); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool capsuleCapsuleDistance( const Capsule& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, diff --git a/src/narrowphase/detail/primitive_shape_algorithm/halfspace.cpp b/src/narrowphase/detail/primitive_shape_algorithm/halfspace.cpp index 045a4da93..0f5b709d0 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/halfspace.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/halfspace.cpp @@ -59,6 +59,7 @@ double halfspaceIntersectTolerance() //============================================================================== template +FCL_INSTANTIATION_DEF_API bool sphereHalfspaceIntersect( const Sphere& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -66,6 +67,7 @@ bool sphereHalfspaceIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool ellipsoidHalfspaceIntersect( const Ellipsoid& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -73,12 +75,14 @@ bool ellipsoidHalfspaceIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool boxHalfspaceIntersect( const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool boxHalfspaceIntersect( const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -86,6 +90,7 @@ bool boxHalfspaceIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool capsuleHalfspaceIntersect( const Capsule& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -93,6 +98,7 @@ bool capsuleHalfspaceIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool cylinderHalfspaceIntersect( const Cylinder& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -100,6 +106,7 @@ bool cylinderHalfspaceIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool coneHalfspaceIntersect( const Cone& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -107,6 +114,7 @@ bool coneHalfspaceIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool convexHalfspaceIntersect( const Convex& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -114,6 +122,7 @@ bool convexHalfspaceIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool halfspaceTriangleIntersect( const Halfspace& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, @@ -121,6 +130,7 @@ bool halfspaceTriangleIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool planeHalfspaceIntersect( const Plane& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -131,6 +141,7 @@ bool planeHalfspaceIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool halfspacePlaneIntersect( const Halfspace& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, @@ -140,6 +151,7 @@ bool halfspacePlaneIntersect( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool halfspaceIntersect( const Halfspace& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, diff --git a/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp b/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp index b4d8a2253..c0ef3efda 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp @@ -1,50 +1,50 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" - -namespace fcl -{ - -namespace detail -{ - -template -class Intersect; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" + +namespace fcl +{ + +namespace detail +{ + +template +class FCL_INSTANTIATION_DEF_API Intersect; + +} // namespace detail +} // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/plane.cpp b/src/narrowphase/detail/primitive_shape_algorithm/plane.cpp index 5cee207ea..1b246caf6 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/plane.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/plane.cpp @@ -59,64 +59,75 @@ float planeIntersectTolerance() //============================================================================== template +FCL_INSTANTIATION_DEF_API bool spherePlaneIntersect(const Sphere& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool boxPlaneIntersect(const Box& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool conePlaneIntersect(const Cone& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, Vector3* contact_points, double* penetration_depth, Vector3* normal); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool planeTriangleIntersect(const Plane& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, Vector3* contact_points, double* penetration_depth, Vector3* normal); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool planeIntersect(const Plane& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp index 291ad3063..34c8aa4d3 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp @@ -43,18 +43,20 @@ namespace detail { //============================================================================== -template bool -sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, - const Box& box, const Transform3& X_FB, - std::vector>* contacts); +template +FCL_INSTANTIATION_DEF_API +bool sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + std::vector>* contacts); //============================================================================== -template bool -sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, - const Box& box, const Transform3& X_FB, - double* distance, Vector3* p_FSb, - Vector3* p_FBs); +template +FCL_INSTANTIATION_DEF_API +bool sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, + const Box& box, const Transform3& X_FB, + double* distance, Vector3* p_FSb, + Vector3* p_FBs); } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.cpp index 90d553d5e..372d94599 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.cpp @@ -45,6 +45,7 @@ namespace detail //============================================================================== template +FCL_INSTANTIATION_DEF_API void lineSegmentPointClosestToPoint( const Vector3 &p, const Vector3 &s1, @@ -53,12 +54,14 @@ void lineSegmentPointClosestToPoint( //============================================================================== template +FCL_INSTANTIATION_DEF_API bool sphereCapsuleIntersect(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp index d9e87d599..0d663b1e5 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp @@ -42,22 +42,24 @@ namespace fcl namespace detail { -template bool -sphereCylinderIntersect(const Sphere& sphere, - const Transform3& X_FS, - const Cylinder& cylinder, - const Transform3& X_FC, - std::vector>* contacts); +template +FCL_INSTANTIATION_DEF_API +bool sphereCylinderIntersect(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + std::vector>* contacts); //============================================================================== -template bool -sphereCylinderDistance(const Sphere& sphere, - const Transform3& X_FS, - const Cylinder& cylinder, - const Transform3& X_FC, - double* distance, Vector3* p_FSc, - Vector3* p_FCs); +template +FCL_INSTANTIATION_DEF_API +bool sphereCylinderDistance(const Sphere& sphere, + const Transform3& X_FS, + const Cylinder& cylinder, + const Transform3& X_FC, + double* distance, Vector3* p_FSc, + Vector3* p_FCs); } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.cpp index e4e76ecc7..df2d0506f 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.cpp @@ -45,12 +45,14 @@ namespace detail //============================================================================== template +FCL_INSTANTIATION_DEF_API bool sphereSphereIntersect(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool sphereSphereDistance(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.cpp index 29b6173be..2a1fb5d1c 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.cpp @@ -45,31 +45,37 @@ namespace detail //============================================================================== template +FCL_INSTANTIATION_DEF_API double segmentSqrDistance(const Vector3& from, const Vector3& to,const Vector3& p, Vector3& nearest); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool projectInTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& normal, const Vector3& p); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool sphereTriangleIntersect(const Sphere& s, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, Vector3* contact_points, double* penetration_depth, Vector3* normal_); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, double* dist); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, double* dist, Vector3* p1, Vector3* p2); //============================================================================== template +FCL_INSTANTIATION_DEF_API bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp b/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp index 2bb3a20b8..62c14a1dc 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp @@ -1,51 +1,51 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -template -class TriangleDistance; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +template +class FCL_INSTANTIATION_DEF_API TriangleDistance; + +} // namespace detail +} // namespace fcl diff --git a/src/narrowphase/detail/traversal/collision/collision_traversal_node_base.cpp b/src/narrowphase/detail/traversal/collision/collision_traversal_node_base.cpp index 89783e3ff..adf3e1b4f 100644 --- a/src/narrowphase/detail/traversal/collision/collision_traversal_node_base.cpp +++ b/src/narrowphase/detail/traversal/collision/collision_traversal_node_base.cpp @@ -44,7 +44,7 @@ namespace detail { template -class CollisionTraversalNodeBase; +class FCL_INSTANTIATION_DEF_API CollisionTraversalNodeBase; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.cpp b/src/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.cpp index 46d34aa59..a4dcd3c38 100644 --- a/src/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.cpp +++ b/src/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.cpp @@ -45,10 +45,11 @@ namespace detail //============================================================================== template -class MeshCollisionTraversalNodeOBB; +class FCL_INSTANTIATION_DEF_API MeshCollisionTraversalNodeOBB; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool initialize( MeshCollisionTraversalNodeOBB& node, const BVHModel>& model1, @@ -60,10 +61,11 @@ bool initialize( //============================================================================== template -class MeshCollisionTraversalNodeRSS; +class FCL_INSTANTIATION_DEF_API MeshCollisionTraversalNodeRSS; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool initialize( MeshCollisionTraversalNodeRSS& node, const BVHModel>& model1, @@ -75,10 +77,11 @@ bool initialize( //============================================================================== template -class MeshCollisionTraversalNodekIOS; +class FCL_INSTANTIATION_DEF_API MeshCollisionTraversalNodekIOS; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool initialize( MeshCollisionTraversalNodekIOS& node, const BVHModel>& model1, @@ -90,10 +93,11 @@ bool initialize( //============================================================================== template -class MeshCollisionTraversalNodeOBBRSS; +class FCL_INSTANTIATION_DEF_API MeshCollisionTraversalNodeOBBRSS; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool initialize( MeshCollisionTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/src/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.cpp b/src/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.cpp index d34c701f0..760ca025c 100644 --- a/src/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.cpp +++ b/src/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.cpp @@ -44,7 +44,7 @@ namespace detail { template -struct BVHContinuousCollisionPair; +struct FCL_INSTANTIATION_DEF_API BVHContinuousCollisionPair; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/collision_node.cpp b/src/narrowphase/detail/traversal/collision_node.cpp index 352cd40af..0fa989540 100644 --- a/src/narrowphase/detail/traversal/collision_node.cpp +++ b/src/narrowphase/detail/traversal/collision_node.cpp @@ -45,22 +45,27 @@ namespace detail //============================================================================== template +FCL_INSTANTIATION_DEF_API void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list); //============================================================================== template +FCL_INSTANTIATION_DEF_API void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list); //============================================================================== template +FCL_INSTANTIATION_DEF_API void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize); //============================================================================== template +FCL_INSTANTIATION_DEF_API void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list); //============================================================================== template +FCL_INSTANTIATION_DEF_API void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list); } // namespace detail diff --git a/src/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.cpp b/src/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.cpp index b2501b6b8..174306910 100644 --- a/src/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.cpp +++ b/src/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.cpp @@ -44,7 +44,7 @@ namespace detail { template -struct ConservativeAdvancementStackData; +struct FCL_INSTANTIATION_DEF_API ConservativeAdvancementStackData; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/distance/distance_traversal_node_base.cpp b/src/narrowphase/detail/traversal/distance/distance_traversal_node_base.cpp index b6cca4994..ab2859c7d 100644 --- a/src/narrowphase/detail/traversal/distance/distance_traversal_node_base.cpp +++ b/src/narrowphase/detail/traversal/distance/distance_traversal_node_base.cpp @@ -44,7 +44,7 @@ namespace detail { template -class DistanceTraversalNodeBase; +class FCL_INSTANTIATION_DEF_API DistanceTraversalNodeBase; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.cpp b/src/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.cpp index ed7d8cc0a..72018ca30 100644 --- a/src/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.cpp +++ b/src/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.cpp @@ -45,10 +45,11 @@ namespace detail //============================================================================== template -class MeshConservativeAdvancementTraversalNodeRSS; +class FCL_INSTANTIATION_DEF_API MeshConservativeAdvancementTraversalNodeRSS; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool initialize( MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel>& model1, @@ -59,10 +60,11 @@ bool initialize( //============================================================================== template -class MeshConservativeAdvancementTraversalNodeOBBRSS; +class FCL_INSTANTIATION_DEF_API MeshConservativeAdvancementTraversalNodeOBBRSS; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool initialize( MeshConservativeAdvancementTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/src/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.cpp b/src/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.cpp index 7d862c143..d27885a21 100644 --- a/src/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.cpp +++ b/src/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.cpp @@ -45,10 +45,11 @@ namespace detail //============================================================================== template -class MeshDistanceTraversalNodeRSS; +class FCL_INSTANTIATION_DEF_API MeshDistanceTraversalNodeRSS; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool initialize( MeshDistanceTraversalNodeRSS& node, const BVHModel>& model1, @@ -60,10 +61,11 @@ bool initialize( //============================================================================== template -class MeshDistanceTraversalNodekIOS; +class FCL_INSTANTIATION_DEF_API MeshDistanceTraversalNodekIOS; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool initialize( MeshDistanceTraversalNodekIOS& node, const BVHModel>& model1, @@ -75,10 +77,11 @@ bool initialize( //============================================================================== template -class MeshDistanceTraversalNodeOBBRSS; +class FCL_INSTANTIATION_DEF_API MeshDistanceTraversalNodeOBBRSS; //============================================================================== template +FCL_INSTANTIATION_DEF_API bool initialize( MeshDistanceTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/src/narrowphase/detail/traversal/traversal_node_base.cpp b/src/narrowphase/detail/traversal/traversal_node_base.cpp index 4a11aaf84..8875b212a 100644 --- a/src/narrowphase/detail/traversal/traversal_node_base.cpp +++ b/src/narrowphase/detail/traversal/traversal_node_base.cpp @@ -44,7 +44,7 @@ namespace detail { template -class TraversalNodeBase; +class FCL_INSTANTIATION_DEF_API TraversalNodeBase; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/traversal_recurse.cpp b/src/narrowphase/detail/traversal/traversal_recurse.cpp index a382a3200..146a7851a 100644 --- a/src/narrowphase/detail/traversal/traversal_recurse.cpp +++ b/src/narrowphase/detail/traversal/traversal_recurse.cpp @@ -45,30 +45,37 @@ namespace detail //============================================================================== template +FCL_INSTANTIATION_DEF_API void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); //============================================================================== template +FCL_INSTANTIATION_DEF_API void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); //============================================================================== template +FCL_INSTANTIATION_DEF_API void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); //============================================================================== template +FCL_INSTANTIATION_DEF_API void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list); //============================================================================== template +FCL_INSTANTIATION_DEF_API void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); //============================================================================== template +FCL_INSTANTIATION_DEF_API void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize); //============================================================================== template +FCL_INSTANTIATION_DEF_API void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list); } // namespace detail diff --git a/src/narrowphase/distance.cpp b/src/narrowphase/distance.cpp index 5ef7dc6be..94d4a1c89 100644 --- a/src/narrowphase/distance.cpp +++ b/src/narrowphase/distance.cpp @@ -42,6 +42,7 @@ namespace fcl //============================================================================== template +FCL_INSTANTIATION_DEF_API double distance( const CollisionObject* o1, const CollisionObject* o2, @@ -50,6 +51,7 @@ double distance( //============================================================================== template +FCL_INSTANTIATION_DEF_API double distance( const CollisionGeometry* o1, const Transform3& tf1, const CollisionGeometry* o2, const Transform3& tf2, diff --git a/src/narrowphase/distance_request.cpp b/src/narrowphase/distance_request.cpp index e6940139b..f06720233 100644 --- a/src/narrowphase/distance_request.cpp +++ b/src/narrowphase/distance_request.cpp @@ -41,6 +41,6 @@ namespace fcl { template -struct DistanceRequest; +struct FCL_INSTANTIATION_DEF_API DistanceRequest; } // namespace fcl diff --git a/src/narrowphase/distance_result.cpp b/src/narrowphase/distance_result.cpp index 1fefd21e8..5171620ca 100644 --- a/src/narrowphase/distance_result.cpp +++ b/src/narrowphase/distance_result.cpp @@ -41,6 +41,6 @@ namespace fcl { template -struct DistanceResult; +struct FCL_INSTANTIATION_DEF_API DistanceResult; } // namespace fcl From 1b5f9eb4e499d1c16f2833a10ef67b979cf9f9ed Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Wed, 26 Jan 2022 11:56:37 +0100 Subject: [PATCH 02/16] one more template to handle --- .../detail/primitive_shape_algorithm/halfspace-inl.h | 4 +++- .../narrowphase/detail/primitive_shape_algorithm/halfspace.h | 2 +- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h index 2a63872b2..51e4dcdd3 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h @@ -110,7 +110,9 @@ bool convexHalfspaceIntersect( Vector3* contact_points, double* penetration_depth, Vector3* normal); //============================================================================== -extern template bool convexHalfspaceIntersect( +extern template +FCL_EXTERN_TEMPLATE_API +bool convexHalfspaceIntersect( const Convex& convex_C, const Transform3& X_FC, const Halfspace& half_space_H, const Transform3& X_FH, std::vector>* contacts); diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h index 0e50539f8..e9e3795b8 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h @@ -151,7 +151,7 @@ bool convexHalfspaceIntersect(const Convex& s1, const Transform3& tf1, /// @return `true` if the two geometries are intersecting. /// @tparam S The computational scalar. template -FCL_EXPORT bool convexHalfspaceIntersect( +bool convexHalfspaceIntersect( const Convex& convex_C, const Transform3& X_FC, const Halfspace& half_space_H, const Transform3& X_FH, std::vector>* contacts); From e61f38e9ed60c771cca7ef6f5431de87bd086640 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Wed, 26 Jan 2022 11:57:26 +0100 Subject: [PATCH 03/16] simplify export macros --- include/fcl/CMakeLists.txt | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/include/fcl/CMakeLists.txt b/include/fcl/CMakeLists.txt index df229bb78..1dab9dc54 100644 --- a/include/fcl/CMakeLists.txt +++ b/include/fcl/CMakeLists.txt @@ -36,24 +36,21 @@ set(GENERATED_FILE_MARKER "GENERATED FILE DO NOT EDIT") configure_file(config.h.in config.h @ONLY) set(FCL_EXPORT_MACRO_NAME "FCL_API") -if(MSVC) - set(EXPORT_INSTANTIATION_TEMPLATE_MACROS " +set(EXPORT_INSTANTIATION_TEMPLATE_MACROS " #ifdef ${PROJECT_NAME}_EXPORTS /* We are building this library */ -# define FCL_EXTERN_TEMPLATE_API -# define FCL_INSTANTIATION_DEF_API ${FCL_EXPORT_MACRO_NAME} +# ifdef _MSC_VER +# define FCL_EXTERN_TEMPLATE_API +# define FCL_INSTANTIATION_DEF_API ${FCL_EXPORT_MACRO_NAME} +# else +# define FCL_EXTERN_TEMPLATE_API ${FCL_EXPORT_MACRO_NAME} +# define FCL_INSTANTIATION_DEF_API +# endif #else /* We are using this library */ # define FCL_EXTERN_TEMPLATE_API ${FCL_EXPORT_MACRO_NAME} -# define FCL_INSTANTIATION_DEF_API #endif ") -else() - set(EXPORT_INSTANTIATION_TEMPLATE_MACROS " -#define FCL_EXTERN_TEMPLATE_API ${FCL_EXPORT_MACRO_NAME} -#define FCL_INSTANTIATION_DEF_API -") -endif() generate_export_header(${PROJECT_NAME} EXPORT_MACRO_NAME ${FCL_EXPORT_MACRO_NAME} EXPORT_FILE_NAME export.h From ca4f5ae9f2ec24caeb860fca73000538b9af58a0 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Wed, 26 Jan 2022 12:56:20 +0100 Subject: [PATCH 04/16] rely on CMake properties to hide all symbols --- src/CMakeLists.txt | 22 +++++----------------- 1 file changed, 5 insertions(+), 17 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 425724e78..a9946be19 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -56,23 +56,11 @@ set_target_properties(${PROJECT_NAME} PROPERTIES # Hide symbols by default ################################################# -# Macro to check for visibility capability in compiler -# Original idea from: https://gitorious.org/ferric-cmake-stuff/ -macro (check_compiler_visibility) - include (CheckCXXCompilerFlag) - check_cxx_compiler_flag(-fvisibility=hidden COMPILER_SUPPORTS_VISIBILITY) -endmacro() - -if (UNIX) - check_compiler_visibility() - if (COMPILER_SUPPORTS_VISIBILITY) - set_target_properties(${PROJECT_NAME} - PROPERTIES COMPILE_FLAGS "-fvisibility=hidden") - endif() -endif() - -if (FCL_HIDE_ALL_SYMBOLS) - add_definitions("-DFCL_STATIC_DEFINE") +if(FCL_HIDE_ALL_SYMBOLS AND NOT FCL_STATIC_LIBRARY) + set_target_properties(${PROJECT_NAME} PROPERTIES + CXX_VISIBILITY_PRESET hidden + VISIBILITY_INLINES_HIDDEN TRUE + ) endif() # Use the IMPORTED target from newer versions of ccd-config.cmake if available, From 8b3fde5a3ce12aca8d76667f95b65c1f8cea935c Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Wed, 26 Jan 2022 13:12:00 +0100 Subject: [PATCH 05/16] allow to build test even if symbols are hidden --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6e5bbb8bb..ad484a800 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -378,7 +378,8 @@ if(NOT FCL_BUILD_TESTS STREQUAL "DEFAULT") unset(_BUILD_TESTING) endif() -if(BUILD_TESTING AND NOT FCL_HIDE_ALL_SYMBOLS) +if(BUILD_TESTING) + enable_testing() add_subdirectory(test) endif() From cd7fdfc8145ad95d3abad8015377a8331bbc4b0e Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Wed, 26 Jan 2022 13:12:35 +0100 Subject: [PATCH 06/16] hide all symbols by default --- CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ad484a800..08f786f41 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,9 +46,7 @@ configure_file(CTestCustom.cmake.in CTestCustom.cmake @ONLY) option(FCL_ENABLE_PROFILING "Enable profiling" OFF) option(FCL_TREAT_WARNINGS_AS_ERRORS "Treat warnings as errors" OFF) -# Option for some bundle-like build system in order not to expose -# any FCL binary symbols in their public ABI -option(FCL_HIDE_ALL_SYMBOLS "Hide all binary symbols" OFF) +option(FCL_HIDE_ALL_SYMBOLS "Hide all binary symbols" ON) # set the default build type if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) From 0d4ca10760997eb617c6913d2d2ddba647e41ae2 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Fri, 28 Jan 2022 15:33:27 +0100 Subject: [PATCH 07/16] always build test_fcl_utility as a static library --- test/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 50e1c1c7b..5ef5ecc0c 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -32,7 +32,7 @@ execute_process(COMMAND cmake -E remove_directory ${CMAKE_BINARY_DIR}/test_resul execute_process(COMMAND cmake -E make_directory ${CMAKE_BINARY_DIR}/test_results) include_directories(${GTEST_INCLUDE_DIRS}) -add_library(test_fcl_utility test_fcl_utility.cpp) +add_library(test_fcl_utility STATIC test_fcl_utility.cpp) target_link_libraries(test_fcl_utility fcl) # test file list From cfdddd90cb8aa500b7b64504571ec2d5124186a9 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Fri, 28 Jan 2022 15:34:00 +0100 Subject: [PATCH 08/16] properly replace warning level /W3 by /W1 or /W4 for msvc --- CMakeModules/CompilerSettings.cmake | 5 ++++- test/gtest/cmake/internal_utils.cmake | 4 +++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/CMakeModules/CompilerSettings.cmake b/CMakeModules/CompilerSettings.cmake index 96b9e1f73..21a526a63 100644 --- a/CMakeModules/CompilerSettings.cmake +++ b/CMakeModules/CompilerSettings.cmake @@ -64,7 +64,10 @@ if(MSVC) if(MSVC_VERSION VERSION_LESS 1900) message(FATAL_ERROR "${PROJECT_NAME} requires Visual Studio 2015 or greater.") endif() - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP /W1 /bigobj") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP /bigobj") + if(CMAKE_CXX_FLAGS MATCHES "/W[0-4]") + STRING(REGEX REPLACE "/W[0-4]" "/W1" CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") + endif() if(FCL_TREAT_WARNINGS_AS_ERRORS) add_definitions(/WX) endif() diff --git a/test/gtest/cmake/internal_utils.cmake b/test/gtest/cmake/internal_utils.cmake index 8cb21894c..5cdea5a12 100644 --- a/test/gtest/cmake/internal_utils.cmake +++ b/test/gtest/cmake/internal_utils.cmake @@ -37,7 +37,9 @@ macro(fix_default_compiler_settings_) # We prefer more strict warning checking for building Google Test. # Replaces /W3 with /W4 in defaults. - string(REPLACE "/W3" "-W4" ${flag_var} "${${flag_var}}") + if(${flag_var} MATCHES "/W[0-4]") + STRING(REGEX REPLACE "/W[0-4]" "/W4" ${flag_var} "${${flag_var}}") + endif() endforeach() endif() endmacro() From f04a3587dc0d131eb0bc497f66c73dbbd9de18ec Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Fri, 28 Jan 2022 16:04:52 +0100 Subject: [PATCH 09/16] add missing destructor, constructors & assignements operators in Contact & ContactPoint --- include/fcl/narrowphase/contact-inl.h | 20 ++++++++++++++++++++ include/fcl/narrowphase/contact.h | 12 +++++++++++- include/fcl/narrowphase/contact_point-inl.h | 20 ++++++++++++++++++++ include/fcl/narrowphase/contact_point.h | 17 ++++++++++++++++- 4 files changed, 67 insertions(+), 2 deletions(-) diff --git a/include/fcl/narrowphase/contact-inl.h b/include/fcl/narrowphase/contact-inl.h index 00fb43c02..4bf729089 100644 --- a/include/fcl/narrowphase/contact-inl.h +++ b/include/fcl/narrowphase/contact-inl.h @@ -58,6 +58,26 @@ Contact::Contact() // Do nothing } +//============================================================================== +template +Contact::~Contact() = default; + +//============================================================================== +template +Contact::Contact(const Contact&) = default; + +//============================================================================== +template +Contact& Contact::operator=(const Contact&) = default; + +//============================================================================== +template +Contact::Contact(Contact&&) noexcept = default; + +//============================================================================== +template +Contact& Contact::operator=(Contact&&) noexcept = default; + //============================================================================== template Contact::Contact( diff --git a/include/fcl/narrowphase/contact.h b/include/fcl/narrowphase/contact.h index 43167de2d..ee3207916 100644 --- a/include/fcl/narrowphase/contact.h +++ b/include/fcl/narrowphase/contact.h @@ -66,7 +66,7 @@ struct Contact /// if object 2 is octree, it is the query cell id (see /// OcTree::getNodeByQueryCellId) intptr_t b2; - + /// @brief contact normal, pointing from o1 to o2 Vector3 normal; @@ -82,6 +82,16 @@ struct Contact Contact(); + ~Contact(); + + Contact(const Contact&); + + Contact& operator=(const Contact&); + + Contact(Contact&&) noexcept; + + Contact& operator=(Contact&&) noexcept; + Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_); Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_, diff --git a/include/fcl/narrowphase/contact_point-inl.h b/include/fcl/narrowphase/contact_point-inl.h index d6fe17b5f..fba2432ae 100644 --- a/include/fcl/narrowphase/contact_point-inl.h +++ b/include/fcl/narrowphase/contact_point-inl.h @@ -68,6 +68,26 @@ ContactPoint::ContactPoint() // Do nothing } +//============================================================================== +template +ContactPoint::~ContactPoint() = default; + +//============================================================================== +template +ContactPoint::ContactPoint(const ContactPoint&) = default; + +//============================================================================== +template +ContactPoint& ContactPoint::operator=(const ContactPoint&) = default; + +//============================================================================== +template +ContactPoint::ContactPoint(ContactPoint&&) noexcept = default; + +//============================================================================== +template +ContactPoint& ContactPoint::operator=(ContactPoint&&) noexcept = default; + //============================================================================== template ContactPoint::ContactPoint( diff --git a/include/fcl/narrowphase/contact_point.h b/include/fcl/narrowphase/contact_point.h index c917d6987..267c9c3ef 100644 --- a/include/fcl/narrowphase/contact_point.h +++ b/include/fcl/narrowphase/contact_point.h @@ -56,9 +56,24 @@ struct ContactPoint /// @brief Penetration depth S penetration_depth; - /// @brief Constructor + /// @brief Default Constructor ContactPoint(); + /// @brief Destructor + ~ContactPoint(); + + /// @brief Copy Constructor + ContactPoint(const ContactPoint&); + + /// @brief Copy assignment operator + ContactPoint& operator=(const ContactPoint&); + + /// @brief Move Constructor + ContactPoint(ContactPoint&&) noexcept; + + /// @brief Move assignment operator + ContactPoint& operator=(ContactPoint&&) noexcept; + /// @brief Constructor ContactPoint(const Vector3& n_, const Vector3& p_, S d_); }; From c30d40fbab2f05cb943b050463cca4dcc4187606 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Fri, 28 Jan 2022 16:05:09 +0100 Subject: [PATCH 10/16] fix test_convex.cpp for msvc --- test/geometry/shape/test_convex.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/test/geometry/shape/test_convex.cpp b/test/geometry/shape/test_convex.cpp index 1857ce36a..41fd6188b 100644 --- a/test/geometry/shape/test_convex.cpp +++ b/test/geometry/shape/test_convex.cpp @@ -46,6 +46,7 @@ #include "eigen_matrix_compare.h" #include "expect_throws_message.h" #include "fcl/common/types.h" +#include "fcl/math/constants.h" namespace fcl { class ConvexTester { @@ -776,8 +777,9 @@ GTEST_TEST(ConvexGeometry, WaterTightValidation) { class TessellatedSphere final : public Polytope { public: TessellatedSphere() : Polytope(1.0) { + const auto pi = constants::pi(); // The angle between the latitude lines measured along the prime meridian. - const double dphi = M_PI / 8; + const double dphi = pi / 8; auto slice_height = [dphi](int slice_index) { // Assumes 1 <= slice_index < 8. return std::cos(slice_index * dphi); @@ -790,7 +792,7 @@ class TessellatedSphere final : public Polytope { vertices_->push_back({0, 0, 1}); // Now create the bands of vertices between slices 1 & 2, 2 & 3, etc. // The angle between the longitude lines measured along the equator. - const double dtheta = 2 * M_PI / 8; + const double dtheta = 2 * pi / 8; for (int slice = 1; slice < 8; ++slice) { double z = slice_height(slice); double r = slice_radius(slice); From 09fecf867721f10803444a8c3a4117e30f49911b Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Fri, 28 Jan 2022 16:14:08 +0100 Subject: [PATCH 11/16] allow to easily run tests on windows --- CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 08f786f41..7d2795958 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,7 +63,11 @@ if(MSVC OR MSVC90 OR MSVC10) set(MSVC ON) endif (MSVC OR MSVC90 OR MSVC10) -set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib) +if(WIN32) + set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/bin) +else() + set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/lib) +endif() set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules") include(CMakePackageConfigHelpers) include(GenerateExportHeader) From af00cc29bffc9eb9a9523651df8bff9818db3d26 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Sat, 29 Jan 2022 02:19:58 +0100 Subject: [PATCH 12/16] fix MinGW --- include/fcl/geometry/shape/utility-inl.h | 1395 ++++++++++++---------- 1 file changed, 746 insertions(+), 649 deletions(-) diff --git a/include/fcl/geometry/shape/utility-inl.h b/include/fcl/geometry/shape/utility-inl.h index e2a1725e1..7b0eb799c 100644 --- a/include/fcl/geometry/shape/utility-inl.h +++ b/include/fcl/geometry/shape/utility-inl.h @@ -135,814 +135,197 @@ void constructBox(const KDOP& bv, const Transform3& tf_bv, B namespace detail { //============================================================================== -//============================================================================== template struct ComputeBVImpl { - static void run(const Shape& s, const Transform3& tf, BV& bv) - { - std::vector> convex_bound_vertices = s.getBoundVertices(tf); - fit(convex_bound_vertices.data(), - static_cast(convex_bound_vertices.size()), bv); - } + static void run(const Shape& s, const Transform3& tf, BV& bv); }; -//============================================================================== template struct ComputeBVImpl, Box> { - static void run(const Box& s, const Transform3& tf, AABB& bv) - { - const Matrix3& R = tf.linear(); - const Vector3& T = tf.translation(); - - S x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2])); - S y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2])); - S z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2])); - - Vector3 v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; - } + static void run(const Box& s, const Transform3& tf, AABB& bv); }; -//============================================================================== template struct ComputeBVImpl, Box> { - static void run(const Box& s, const Transform3& tf, OBB& bv) - { - bv.axis = tf.linear(); - bv.To = tf.translation(); - bv.extent = s.side * (S)0.5; - } + static void run(const Box& s, const Transform3& tf, OBB& bv); }; -//============================================================================== template struct ComputeBVImpl, Capsule> { - static void run(const Capsule& s, const Transform3& tf, AABB& bv) - { - const Matrix3& R = tf.linear(); - const Vector3& T = tf.translation(); - - S x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius; - S y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius; - S z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius; - - Vector3 v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; - } + static void run(const Capsule& s, const Transform3& tf, AABB& bv); }; -//============================================================================== template struct ComputeBVImpl, Capsule> { - static void run(const Capsule& s, const Transform3& tf, OBB& bv) - { - bv.axis = tf.linear(); - bv.To = tf.translation(); - bv.extent << s.radius, s.radius, s.lz / 2 + s.radius; - } + static void run(const Capsule& s, const Transform3& tf, OBB& bv); }; -//============================================================================== template struct ComputeBVImpl, Cone> { - static void run(const Cone& s, const Transform3& tf, AABB& bv) - { - const Matrix3& R = tf.linear(); - const Vector3& T = tf.translation(); - - S x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); - S y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); - S z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); - - Vector3 v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; - } + static void run(const Cone& s, const Transform3& tf, AABB& bv); }; -//============================================================================== template struct ComputeBVImpl, Cone> { - static void run(const Cone& s, const Transform3& tf, OBB& bv) - { - bv.axis = tf.linear(); - bv.To = tf.translation(); - bv.extent << s.radius, s.radius, s.lz / 2; - } + static void run(const Cone& s, const Transform3& tf, OBB& bv); }; -//============================================================================== template struct ComputeBVImpl, Convex> { - static void run(const Convex& s, const Transform3& tf, AABB& bv) - { - const Matrix3& R = tf.linear(); - const Vector3& T = tf.translation(); - - AABB bv_; - for (const auto& vertex : s.getVertices()) - { - Vector3 new_p = R * vertex + T; - bv_ += new_p; - } - - bv = bv_; - } + static void run(const Convex& s, const Transform3& tf, AABB& bv); }; -//============================================================================== template struct ComputeBVImpl, Convex> { - static void run(const Convex& s, const Transform3& tf, OBB& bv) - { - fit(s.getVertices().data(), static_cast(s.getVertices().size()), bv); - - bv.axis = tf.linear(); - bv.To = tf * bv.To; - } + static void run(const Convex& s, const Transform3& tf, OBB& bv); }; -//============================================================================== template struct ComputeBVImpl, Cylinder> { - static void run(const Cylinder& s, const Transform3& tf, AABB& bv) - { - const Matrix3& R = tf.linear(); - const Vector3& T = tf.translation(); - - S x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); - S y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); - S z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); - - Vector3 v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; - } + static void run(const Cylinder& s, const Transform3& tf, AABB& bv); }; -//============================================================================== template struct ComputeBVImpl, Cylinder> { - static void run(const Cylinder& s, const Transform3& tf, OBB& bv) - { - bv.axis = tf.linear(); - bv.To = tf.translation(); - bv.extent << s.radius, s.radius, s.lz / 2; - } + static void run(const Cylinder& s, const Transform3& tf, OBB& bv); }; -//============================================================================== template struct ComputeBVImpl, Ellipsoid> { - static void run(const Ellipsoid& s, const Transform3& tf, AABB& bv) - { - const Matrix3& R = tf.linear(); - const Vector3& T = tf.translation(); - - S x_range = (fabs(R(0, 0) * s.radii[0]) + fabs(R(0, 1) * s.radii[1]) + fabs(R(0, 2) * s.radii[2])); - S y_range = (fabs(R(1, 0) * s.radii[0]) + fabs(R(1, 1) * s.radii[1]) + fabs(R(1, 2) * s.radii[2])); - S z_range = (fabs(R(2, 0) * s.radii[0]) + fabs(R(2, 1) * s.radii[1]) + fabs(R(2, 2) * s.radii[2])); - - Vector3 v_delta(x_range, y_range, z_range); - bv.max_ = T + v_delta; - bv.min_ = T - v_delta; - } + static void run(const Ellipsoid& s, const Transform3& tf, AABB& bv); }; -//============================================================================== template struct ComputeBVImpl, Ellipsoid> { - static void run(const Ellipsoid& s, const Transform3& tf, OBB& bv) - { - bv.axis = tf.linear(); - bv.To = tf.translation(); - bv.extent = s.radii; - } + static void run(const Ellipsoid& s, const Transform3& tf, OBB& bv); }; -//============================================================================== template struct ComputeBVImpl, Halfspace> { - static void run(const Halfspace& s, const Transform3& tf, AABB& bv) - { - Halfspace new_s = transform(s, tf); - const Vector3& n = new_s.n; - const S& d = new_s.d; - - AABB bv_; - bv_.min_ = Vector3::Constant(-std::numeric_limits::max()); - bv_.max_ = Vector3::Constant(std::numeric_limits::max()); - if(n[1] == (S)0.0 && n[2] == (S)0.0) - { - // normal aligned with x axis - if(n[0] < 0) bv_.min_[0] = -d; - else if(n[0] > 0) bv_.max_[0] = d; - } - else if(n[0] == (S)0.0 && n[2] == (S)0.0) - { - // normal aligned with y axis - if(n[1] < 0) bv_.min_[1] = -d; - else if(n[1] > 0) bv_.max_[1] = d; - } - else if(n[0] == (S)0.0 && n[1] == (S)0.0) - { - // normal aligned with z axis - if(n[2] < 0) bv_.min_[2] = -d; - else if(n[2] > 0) bv_.max_[2] = d; - } - - bv = bv_; - } + static void run(const Halfspace& s, const Transform3& tf, AABB& bv); }; -//============================================================================== template struct ComputeBVImpl, Halfspace> { - static void run(const Halfspace& s, const Transform3& tf, OBB& bv) - { - FCL_UNUSED(s); - FCL_UNUSED(tf); - - /// Half space can only have very rough OBB - bv.axis.setIdentity(); - bv.To.setZero(); - bv.extent.setConstant(std::numeric_limits::max()); - } + static void run(const Halfspace& s, const Transform3& tf, OBB& bv); }; -//============================================================================== template struct ComputeBVImpl, Halfspace> { - static void run(const Halfspace& s, const Transform3& tf, RSS& bv) - { - FCL_UNUSED(s); - FCL_UNUSED(tf); - - /// Half space can only have very rough RSS - bv.axis.setIdentity(); - bv.To.setZero(); - bv.l[0] = bv.l[1] = bv.r = std::numeric_limits::max(); - } + static void run(const Halfspace& s, const Transform3& tf, RSS& bv); }; -//============================================================================== template struct ComputeBVImpl, Halfspace> { - static void run(const Halfspace& s, const Transform3& tf, OBBRSS& bv) - { - computeBV(s, tf, bv.obb); - computeBV(s, tf, bv.rss); - } + static void run(const Halfspace& s, const Transform3& tf, OBBRSS& bv); }; -//============================================================================== template struct ComputeBVImpl, Halfspace> { - static void run(const Halfspace& s, const Transform3& tf, kIOS& bv) - { - bv.num_spheres = 1; - computeBV(s, tf, bv.obb); - bv.spheres[0].o.setZero(); - bv.spheres[0].r = std::numeric_limits::max(); - } + static void run(const Halfspace& s, const Transform3& tf, kIOS& bv); }; -//============================================================================== template struct ComputeBVImpl, Halfspace> { - static void run(const Halfspace& s, const Transform3& tf, KDOP& bv) - { - Halfspace new_s = transform(s, tf); - const Vector3& n = new_s.n; - const S& d = new_s.d; - - const std::size_t D = 8; - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (S)0.0 && n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(D) = d; - else bv.dist(0) = -d; - } - else if(n[0] == (S)0.0 && n[2] == (S)0.0) - { - if(n[1] > 0) bv.dist(D + 1) = d; - else bv.dist(1) = -d; - } - else if(n[0] == (S)0.0 && n[1] == (S)0.0) - { - if(n[2] > 0) bv.dist(D + 2) = d; - else bv.dist(2) = -d; - } - else if(n[2] == (S)0.0 && n[0] == n[1]) - { - if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; - else bv.dist(3) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] == n[2]) - { - if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; - else bv.dist(4) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] == n[2]) - { - if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; - else bv.dist(5) = n[1] * d * 2; - } - else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) - { - if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; - else bv.dist(6) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; - else bv.dist(7) = n[0] * d * 2; - } - } + static void run(const Halfspace& s, const Transform3& tf, KDOP& bv); }; -//============================================================================== template struct ComputeBVImpl, Halfspace> { - static void run(const Halfspace& s, const Transform3& tf, KDOP& bv) - { - Halfspace new_s = transform(s, tf); - const Vector3& n = new_s.n; - const S& d = new_s.d; - - const std::size_t D = 9; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (S)0.0 && n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(D) = d; - else bv.dist(0) = -d; - } - else if(n[0] == (S)0.0 && n[2] == (S)0.0) - { - if(n[1] > 0) bv.dist(D + 1) = d; - else bv.dist(1) = -d; - } - else if(n[0] == (S)0.0 && n[1] == (S)0.0) - { - if(n[2] > 0) bv.dist(D + 2) = d; - else bv.dist(2) = -d; - } - else if(n[2] == (S)0.0 && n[0] == n[1]) - { - if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; - else bv.dist(3) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] == n[2]) - { - if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; - else bv.dist(4) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] == n[2]) - { - if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; - else bv.dist(5) = n[1] * d * 2; - } - else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) - { - if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; - else bv.dist(6) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; - else bv.dist(7) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) - { - if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; - else bv.dist(8) = n[1] * d * 2; - } - } + static void run(const Halfspace& s, const Transform3& tf, KDOP& bv); }; //============================================================================== template struct ComputeBVImpl, Halfspace> { - static void run(const Halfspace& s, const Transform3& tf, KDOP& bv) - { - Halfspace new_s = transform(s, tf); - const Vector3& n = new_s.n; - const S& d = new_s.d; - - const std::size_t D = 12; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (S)0.0 && n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(D) = d; - else bv.dist(0) = -d; - } - else if(n[0] == (S)0.0 && n[2] == (S)0.0) - { - if(n[1] > 0) bv.dist(D + 1) = d; - else bv.dist(1) = -d; - } - else if(n[0] == (S)0.0 && n[1] == (S)0.0) - { - if(n[2] > 0) bv.dist(D + 2) = d; - else bv.dist(2) = -d; - } - else if(n[2] == (S)0.0 && n[0] == n[1]) - { - if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; - else bv.dist(3) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] == n[2]) - { - if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; - else bv.dist(4) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] == n[2]) - { - if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; - else bv.dist(5) = n[1] * d * 2; - } - else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) - { - if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; - else bv.dist(6) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; - else bv.dist(7) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) - { - if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; - else bv.dist(8) = n[1] * d * 2; - } - else if(n[0] + n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) - { - if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3; - else bv.dist(9) = n[0] * d * 3; - } - else if(n[0] + n[1] == (S)0.0 && n[1] + n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3; - else bv.dist(10) = n[0] * d * 3; - } - else if(n[0] + n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) - { - if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3; - else bv.dist(11) = n[1] * d * 3; - } - } + static void run(const Halfspace& s, const Transform3& tf, KDOP& bv); }; -//============================================================================== template struct ComputeBVImpl, Plane> { - static void run(const Plane& s, const Transform3& tf, AABB& bv) - { - Plane new_s = transform(s, tf); - const Vector3& n = new_s.n; - const S& d = new_s.d; - - AABB bv_; - bv_.min_ = Vector3::Constant(-std::numeric_limits::max()); - bv_.max_ = Vector3::Constant(std::numeric_limits::max()); - if(n[1] == (S)0.0 && n[2] == (S)0.0) - { - // normal aligned with x axis - if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; } - else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; } - } - else if(n[0] == (S)0.0 && n[2] == (S)0.0) - { - // normal aligned with y axis - if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; } - else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; } - } - else if(n[0] == (S)0.0 && n[1] == (S)0.0) - { - // normal aligned with z axis - if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; } - else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; } - } - - bv = bv_; - } + static void run(const Plane& s, const Transform3& tf, AABB& bv); }; -//============================================================================== template struct ComputeBVImpl, Plane> { - static void run(const Plane& s, const Transform3& tf, OBB& bv) - { - const Vector3 n = tf.linear() * s.n; - bv.axis = generateCoordinateSystem(n); - - bv.extent << 0, std::numeric_limits::max(), std::numeric_limits::max(); - - Vector3 p = s.n * s.d; - bv.To = tf * p; /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T - } + static void run(const Plane& s, const Transform3& tf, OBB& bv); }; -//============================================================================== template struct ComputeBVImpl, Plane> { - static void run(const Plane& s, const Transform3& tf, RSS& bv) - { - const Vector3 n = tf.linear() * s.n; - bv.axis = generateCoordinateSystem(n); - - bv.l[0] = std::numeric_limits::max(); - bv.l[1] = std::numeric_limits::max(); - - bv.r = 0; - - Vector3 p = s.n * s.d; - bv.To = tf * p; - } + static void run(const Plane& s, const Transform3& tf, RSS& bv); }; -//============================================================================== template struct ComputeBVImpl, Plane> { - static void run(const Plane& s, const Transform3& tf, OBBRSS& bv) - { - computeBV(s, tf, bv.obb); - computeBV(s, tf, bv.rss); - } + static void run(const Plane& s, const Transform3& tf, OBBRSS& bv); }; -//============================================================================== template struct ComputeBVImpl, Plane> { - static void run(const Plane& s, const Transform3& tf, kIOS& bv) - { - bv.num_spheres = 1; - computeBV(s, tf, bv.obb); - bv.spheres[0].o.setZero(); - bv.spheres[0].r = std::numeric_limits::max(); - } + static void run(const Plane& s, const Transform3& tf, kIOS& bv); }; -//============================================================================== template struct ComputeBVImpl, Plane> { - static void run(const Plane& s, const Transform3& tf, KDOP& bv) - { - Plane new_s = transform(s, tf); - const Vector3& n = new_s.n; - const S& d = new_s.d; - - const std::size_t D = 8; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (S)0.0 && n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; - else bv.dist(0) = bv.dist(D) = -d; - } - else if(n[0] == (S)0.0 && n[2] == (S)0.0) - { - if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; - else bv.dist(1) = bv.dist(D + 1) = -d; - } - else if(n[0] == (S)0.0 && n[1] == (S)0.0) - { - if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; - else bv.dist(2) = bv.dist(D + 2) = -d; - } - else if(n[2] == (S)0.0 && n[0] == n[1]) - { - bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] == n[2]) - { - bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] == n[2]) - { - bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; - } - else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) - { - bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) - { - bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } - } + static void run(const Plane& s, const Transform3& tf, KDOP& bv); }; -//============================================================================== template struct ComputeBVImpl, Plane> { - static void run(const Plane& s, const Transform3& tf, KDOP& bv) - { - Plane new_s = transform(s, tf); - const Vector3& n = new_s.n; - const S& d = new_s.d; - - const std::size_t D = 9; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (S)0.0 && n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; - else bv.dist(0) = bv.dist(D) = -d; - } - else if(n[0] == (S)0.0 && n[2] == (S)0.0) - { - if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; - else bv.dist(1) = bv.dist(D + 1) = -d; - } - else if(n[0] == (S)0.0 && n[1] == (S)0.0) - { - if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; - else bv.dist(2) = bv.dist(D + 2) = -d; - } - else if(n[2] == (S)0.0 && n[0] == n[1]) - { - bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] == n[2]) - { - bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] == n[2]) - { - bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } - else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) - { - bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) - { - bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) - { - bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; - } - } + static void run(const Plane& s, const Transform3& tf, KDOP& bv); }; -//============================================================================== template struct ComputeBVImpl, Plane> { - static void run(const Plane& s, const Transform3& tf, KDOP& bv) - { - Plane new_s = transform(s, tf); - const Vector3& n = new_s.n; - const S& d = new_s.d; - - const std::size_t D = 12; - - for(std::size_t i = 0; i < D; ++i) - bv.dist(i) = -std::numeric_limits::max(); - for(std::size_t i = D; i < 2 * D; ++i) - bv.dist(i) = std::numeric_limits::max(); - - if(n[1] == (S)0.0 && n[2] == (S)0.0) - { - if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; - else bv.dist(0) = bv.dist(D) = -d; - } - else if(n[0] == (S)0.0 && n[2] == (S)0.0) - { - if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; - else bv.dist(1) = bv.dist(D + 1) = -d; - } - else if(n[0] == (S)0.0 && n[1] == (S)0.0) - { - if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; - else bv.dist(2) = bv.dist(D + 2) = -d; - } - else if(n[2] == (S)0.0 && n[0] == n[1]) - { - bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] == n[2]) - { - bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] == n[2]) - { - bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; - } - else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) - { - bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; - } - else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) - { - bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; - } - else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) - { - bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; - } - else if(n[0] + n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) - { - bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; - } - else if(n[0] + n[1] == (S)0.0 && n[1] + n[2] == (S)0.0) - { - bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; - } - else if(n[0] + n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) - { - bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; - } - } + static void run(const Plane& s, const Transform3& tf, KDOP& bv); }; -//============================================================================== template struct ComputeBVImpl, Sphere> { - static void run(const Sphere& s, const Transform3& tf, AABB& bv) - { - const Vector3 v_delta = Vector3::Constant(s.radius); - bv.max_ = tf.translation() + v_delta; - bv.min_ = tf.translation() - v_delta; - } + static void run(const Sphere& s, const Transform3& tf, AABB& bv); }; -//============================================================================== template struct ComputeBVImpl, Sphere> { - static void run(const Sphere& s, const Transform3& tf, OBB& bv) - { - bv.To = tf.translation(); - bv.axis.setIdentity(); - bv.extent.setConstant(s.radius); - } + static void run(const Sphere& s, const Transform3& tf, OBB& bv); }; -//============================================================================== template struct ComputeBVImpl, TriangleP> { - static void run(const TriangleP& s, const Transform3& tf, AABB& bv) - { - bv = AABB(tf * s.a, tf * s.b, tf * s.c); - } + static void run(const TriangleP& s, const Transform3& tf, AABB& bv); }; //============================================================================== @@ -1061,6 +444,720 @@ struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Sphere extern template struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, TriangleP>; +//============================================================================== +template +void ComputeBVImpl::run(const Shape& s, const Transform3& tf, BV& bv) +{ + std::vector> convex_bound_vertices = s.getBoundVertices(tf); + fit(convex_bound_vertices.data(), + static_cast(convex_bound_vertices.size()), bv); +} + +//============================================================================== +template +void ComputeBVImpl, Box>::run(const Box& s, const Transform3& tf, AABB& bv) +{ + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + S x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2])); + S y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2])); + S z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2])); + + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; +} + +//============================================================================== +template +void ComputeBVImpl, Box>::run(const Box& s, const Transform3& tf, OBB& bv) +{ + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent = s.side * (S)0.5; +} + +//============================================================================== +template +void ComputeBVImpl, Capsule>::run(const Capsule& s, const Transform3& tf, AABB& bv) +{ + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + S x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius; + S y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius; + S z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius; + + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; +} + +//============================================================================== +template +void ComputeBVImpl, Capsule>::run(const Capsule& s, const Transform3& tf, OBB& bv) +{ + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent << s.radius, s.radius, s.lz / 2 + s.radius; +} + +//============================================================================== +template +void ComputeBVImpl, Cone>::run(const Cone& s, const Transform3& tf, AABB& bv) +{ + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + S x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); + S y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); + S z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); + + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; +} + +//============================================================================== +template +void ComputeBVImpl, Cone>::run(const Cone& s, const Transform3& tf, OBB& bv) +{ + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent << s.radius, s.radius, s.lz / 2; +} + +//============================================================================== +template +void ComputeBVImpl, Convex>::run(const Convex& s, const Transform3& tf, AABB& bv) +{ + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + AABB bv_; + for (const auto& vertex : s.getVertices()) + { + Vector3 new_p = R * vertex + T; + bv_ += new_p; + } + + bv = bv_; +} + +//============================================================================== +template +void ComputeBVImpl, Convex>::run(const Convex& s, const Transform3& tf, OBB& bv) +{ + fit(s.getVertices().data(), static_cast(s.getVertices().size()), bv); + + bv.axis = tf.linear(); + bv.To = tf * bv.To; +} + +//============================================================================== +template +void ComputeBVImpl, Cylinder>::run(const Cylinder& s, const Transform3& tf, AABB& bv) +{ + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + S x_range = fabs(R(0, 0) * s.radius) + fabs(R(0, 1) * s.radius) + 0.5 * fabs(R(0, 2) * s.lz); + S y_range = fabs(R(1, 0) * s.radius) + fabs(R(1, 1) * s.radius) + 0.5 * fabs(R(1, 2) * s.lz); + S z_range = fabs(R(2, 0) * s.radius) + fabs(R(2, 1) * s.radius) + 0.5 * fabs(R(2, 2) * s.lz); + + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; +} + +//============================================================================== +template +void ComputeBVImpl, Cylinder>::run(const Cylinder& s, const Transform3& tf, OBB& bv) +{ + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent << s.radius, s.radius, s.lz / 2; +} + +//============================================================================== +template +void ComputeBVImpl, Ellipsoid>::run(const Ellipsoid& s, const Transform3& tf, AABB& bv) +{ + const Matrix3& R = tf.linear(); + const Vector3& T = tf.translation(); + + S x_range = (fabs(R(0, 0) * s.radii[0]) + fabs(R(0, 1) * s.radii[1]) + fabs(R(0, 2) * s.radii[2])); + S y_range = (fabs(R(1, 0) * s.radii[0]) + fabs(R(1, 1) * s.radii[1]) + fabs(R(1, 2) * s.radii[2])); + S z_range = (fabs(R(2, 0) * s.radii[0]) + fabs(R(2, 1) * s.radii[1]) + fabs(R(2, 2) * s.radii[2])); + + Vector3 v_delta(x_range, y_range, z_range); + bv.max_ = T + v_delta; + bv.min_ = T - v_delta; +} + +//============================================================================== +template +void ComputeBVImpl, Ellipsoid>::run(const Ellipsoid& s, const Transform3& tf, OBB& bv) +{ + bv.axis = tf.linear(); + bv.To = tf.translation(); + bv.extent = s.radii; +} + +//============================================================================== +template +void ComputeBVImpl, Halfspace>::run(const Halfspace& s, const Transform3& tf, AABB& bv) +{ + Halfspace new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + AABB bv_; + bv_.min_ = Vector3::Constant(-std::numeric_limits::max()); + bv_.max_ = Vector3::Constant(std::numeric_limits::max()); + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + // normal aligned with x axis + if(n[0] < 0) bv_.min_[0] = -d; + else if(n[0] > 0) bv_.max_[0] = d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + // normal aligned with y axis + if(n[1] < 0) bv_.min_[1] = -d; + else if(n[1] > 0) bv_.max_[1] = d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + // normal aligned with z axis + if(n[2] < 0) bv_.min_[2] = -d; + else if(n[2] > 0) bv_.max_[2] = d; + } + + bv = bv_; +} + +//============================================================================== +template +void ComputeBVImpl, Halfspace>::run(const Halfspace& s, const Transform3& tf, OBB& bv) +{ + FCL_UNUSED(s); + FCL_UNUSED(tf); + + /// Half space can only have very rough OBB + bv.axis.setIdentity(); + bv.To.setZero(); + bv.extent.setConstant(std::numeric_limits::max()); +} + +//============================================================================== +template +void ComputeBVImpl, Halfspace>::run(const Halfspace& s, const Transform3& tf, RSS& bv) +{ + FCL_UNUSED(s); + FCL_UNUSED(tf); + + /// Half space can only have very rough RSS + bv.axis.setIdentity(); + bv.To.setZero(); + bv.l[0] = bv.l[1] = bv.r = std::numeric_limits::max(); +} + +//============================================================================== +template +void ComputeBVImpl, Halfspace>::run(const Halfspace& s, const Transform3& tf, OBBRSS& bv) +{ + computeBV(s, tf, bv.obb); + computeBV(s, tf, bv.rss); +} + +//============================================================================== +template +void ComputeBVImpl, Halfspace>::run(const Halfspace& s, const Transform3& tf, kIOS& bv) +{ + bv.num_spheres = 1; + computeBV(s, tf, bv.obb); + bv.spheres[0].o.setZero(); + bv.spheres[0].r = std::numeric_limits::max(); +} + +//============================================================================== +template +void ComputeBVImpl, Halfspace>::run(const Halfspace& s, const Transform3& tf, KDOP& bv) +{ + Halfspace new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 8; + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D) = d; + else bv.dist(0) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 1) = d; + else bv.dist(1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(D + 2) = d; + else bv.dist(2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; + else bv.dist(3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; + else bv.dist(4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; + else bv.dist(5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; + else bv.dist(6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; + else bv.dist(7) = n[0] * d * 2; + } +} + +//============================================================================== +template +void ComputeBVImpl, Halfspace>::run(const Halfspace& s, const Transform3& tf, KDOP& bv) +{ + Halfspace new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 9; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D) = d; + else bv.dist(0) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 1) = d; + else bv.dist(1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(D + 2) = d; + else bv.dist(2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; + else bv.dist(3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; + else bv.dist(4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; + else bv.dist(5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; + else bv.dist(6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; + else bv.dist(7) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; + else bv.dist(8) = n[1] * d * 2; + } +} + +//============================================================================== +template +void ComputeBVImpl, Halfspace>::run(const Halfspace& s, const Transform3& tf, KDOP& bv) +{ + Halfspace new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 12; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D) = d; + else bv.dist(0) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 1) = d; + else bv.dist(1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(D + 2) = d; + else bv.dist(2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2; + else bv.dist(3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2; + else bv.dist(4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2; + else bv.dist(5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2; + else bv.dist(6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2; + else bv.dist(7) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2; + else bv.dist(8) = n[1] * d * 2; + } + else if(n[0] + n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3; + else bv.dist(9) = n[0] * d * 3; + } + else if(n[0] + n[1] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3; + else bv.dist(10) = n[0] * d * 3; + } + else if(n[0] + n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3; + else bv.dist(11) = n[1] * d * 3; + } +} + +//============================================================================== +template +void ComputeBVImpl, Plane>::run(const Plane& s, const Transform3& tf, AABB& bv) +{ + Plane new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + AABB bv_; + bv_.min_ = Vector3::Constant(-std::numeric_limits::max()); + bv_.max_ = Vector3::Constant(std::numeric_limits::max()); + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + // normal aligned with x axis + if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; } + else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; } + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + // normal aligned with y axis + if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; } + else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; } + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + // normal aligned with z axis + if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; } + else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; } + } + + bv = bv_; +} + +//============================================================================== +template +void ComputeBVImpl, Plane>::run(const Plane& s, const Transform3& tf, OBB& bv) +{ + const Vector3 n = tf.linear() * s.n; + bv.axis = generateCoordinateSystem(n); + + bv.extent << 0, std::numeric_limits::max(), std::numeric_limits::max(); + + Vector3 p = s.n * s.d; + bv.To = tf * p; /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T +} + +//============================================================================== +template +void ComputeBVImpl, Plane>::run(const Plane& s, const Transform3& tf, RSS& bv) +{ + const Vector3 n = tf.linear() * s.n; + bv.axis = generateCoordinateSystem(n); + + bv.l[0] = std::numeric_limits::max(); + bv.l[1] = std::numeric_limits::max(); + + bv.r = 0; + + Vector3 p = s.n * s.d; + bv.To = tf * p; +} + +//============================================================================== +template +void ComputeBVImpl, Plane>::run(const Plane& s, const Transform3& tf, OBBRSS& bv) +{ + computeBV(s, tf, bv.obb); + computeBV(s, tf, bv.rss); +} + +//============================================================================== +template +void ComputeBVImpl, Plane>::run(const Plane& s, const Transform3& tf, kIOS& bv) +{ + bv.num_spheres = 1; + computeBV(s, tf, bv.obb); + bv.spheres[0].o.setZero(); + bv.spheres[0].r = std::numeric_limits::max(); +} + +//============================================================================== +template +void ComputeBVImpl, Plane>::run(const Plane& s, const Transform3& tf, KDOP& bv) +{ + Plane new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 8; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; + else bv.dist(0) = bv.dist(D) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; + else bv.dist(1) = bv.dist(D + 1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; + else bv.dist(2) = bv.dist(D + 2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; + } +} + +//============================================================================== +template +void ComputeBVImpl, Plane>::run(const Plane& s, const Transform3& tf, KDOP& bv) +{ + Plane new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 9; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; + else bv.dist(0) = bv.dist(D) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; + else bv.dist(1) = bv.dist(D + 1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; + else bv.dist(2) = bv.dist(D + 2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; + } +} + +//============================================================================== +template +void ComputeBVImpl, Plane>::run(const Plane& s, const Transform3& tf, KDOP& bv) +{ + Plane new_s = transform(s, tf); + const Vector3& n = new_s.n; + const S& d = new_s.d; + + const std::size_t D = 12; + + for(std::size_t i = 0; i < D; ++i) + bv.dist(i) = -std::numeric_limits::max(); + for(std::size_t i = D; i < 2 * D; ++i) + bv.dist(i) = std::numeric_limits::max(); + + if(n[1] == (S)0.0 && n[2] == (S)0.0) + { + if(n[0] > 0) bv.dist(0) = bv.dist(D) = d; + else bv.dist(0) = bv.dist(D) = -d; + } + else if(n[0] == (S)0.0 && n[2] == (S)0.0) + { + if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d; + else bv.dist(1) = bv.dist(D + 1) = -d; + } + else if(n[0] == (S)0.0 && n[1] == (S)0.0) + { + if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d; + else bv.dist(2) = bv.dist(D + 2) = -d; + } + else if(n[2] == (S)0.0 && n[0] == n[1]) + { + bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] == n[2]) + { + bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] == n[2]) + { + bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2; + } + else if(n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2; + } + else if(n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2; + } + else if(n[0] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2; + } + else if(n[0] + n[2] == (S)0.0 && n[0] + n[1] == (S)0.0) + { + bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3; + } + else if(n[0] + n[1] == (S)0.0 && n[1] + n[2] == (S)0.0) + { + bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3; + } + else if(n[0] + n[1] == (S)0.0 && n[0] + n[2] == (S)0.0) + { + bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3; + } +} + +//============================================================================== +template +void ComputeBVImpl, Sphere>::run(const Sphere& s, const Transform3& tf, AABB& bv) +{ + const Vector3 v_delta = Vector3::Constant(s.radius); + bv.max_ = tf.translation() + v_delta; + bv.min_ = tf.translation() - v_delta; +} + +//============================================================================== +template +void ComputeBVImpl, Sphere>::run(const Sphere& s, const Transform3& tf, OBB& bv) +{ + bv.To = tf.translation(); + bv.axis.setIdentity(); + bv.extent.setConstant(s.radius); +} + +//============================================================================== +template +void ComputeBVImpl, TriangleP>::run(const TriangleP& s, const Transform3& tf, AABB& bv) +{ + bv = AABB(tf * s.a, tf * s.b, tf * s.c); +} + //============================================================================== } // namespace detail //============================================================================== From e9b313be7bad9892613d229e386fe768cdf78438 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Sat, 29 Jan 2022 14:28:45 +0100 Subject: [PATCH 13/16] revert autoformatting --- include/fcl/broadphase/broadphase_SSaP-inl.h | 2 +- include/fcl/broadphase/broadphase_SSaP.h | 10 +- include/fcl/broadphase/broadphase_SaP-inl.h | 2 +- include/fcl/broadphase/broadphase_SaP.h | 6 +- .../broadphase/broadphase_bruteforce-inl.h | 2 +- .../fcl/broadphase/broadphase_bruteforce.h | 4 +- .../broadphase/broadphase_collision_manager.h | 4 +- .../broadphase_continuous_collision_manager.h | 4 +- .../broadphase_dynamic_AABB_tree-inl.h | 2 +- .../broadphase/broadphase_dynamic_AABB_tree.h | 8 +- .../broadphase_dynamic_AABB_tree_array-inl.h | 2 +- .../broadphase_dynamic_AABB_tree_array.h | 10 +- .../broadphase/broadphase_interval_tree-inl.h | 2 +- .../fcl/broadphase/broadphase_interval_tree.h | 4 +- .../broadphase/broadphase_spatialhash-inl.h | 2 +- .../fcl/broadphase/broadphase_spatialhash.h | 2 +- .../fcl/broadphase/detail/hierarchy_tree.h | 50 +- .../broadphase/detail/hierarchy_tree_array.h | 44 +- include/fcl/broadphase/detail/interval_tree.h | 2 +- .../broadphase/detail/interval_tree_node.h | 4 +- .../fcl/broadphase/detail/simple_interval.h | 2 +- .../fcl/broadphase/detail/sparse_hash_table.h | 4 +- .../fcl/broadphase/detail/spatial_hash-inl.h | 2 +- include/fcl/broadphase/detail/spatial_hash.h | 4 +- include/fcl/geometry/bvh/BVH_model.h | 6 +- include/fcl/geometry/bvh/BV_node_base.h | 2 +- include/fcl/geometry/octree/octree.h | 2 +- include/fcl/geometry/shape/capsule.h | 8 +- include/fcl/geometry/shape/cone.h | 8 +- include/fcl/geometry/shape/convex.h | 2 +- include/fcl/geometry/shape/cylinder.h | 12 +- include/fcl/geometry/shape/halfspace.h | 4 +- include/fcl/geometry/shape/plane.h | 18 +- include/fcl/geometry/shape/triangle_p.h | 2 +- include/fcl/math/bv/OBB.h | 6 +- include/fcl/math/bv/kDOP.h | 4 +- include/fcl/math/bv/kIOS.h | 2 +- include/fcl/math/detail/polysolver-inl.h | 402 +-- include/fcl/math/detail/polysolver.h | 160 +- include/fcl/math/detail/project-inl.h | 640 ++--- include/fcl/math/detail/project.h | 192 +- include/fcl/math/motion/interp_motion.h | 6 +- include/fcl/math/motion/spline_motion.h | 6 +- .../motion/taylor_model/interval_vector.h | 4 +- .../math/motion/taylor_model/taylor_matrix.h | 2 +- .../math/motion/taylor_model/taylor_model.h | 4 +- .../math/motion/taylor_model/taylor_vector.h | 4 +- .../fcl/math/sampler/sampler_se3_euler_ball.h | 2 +- include/fcl/narrowphase/collision_request.h | 2 +- include/fcl/narrowphase/collision_result.h | 2 +- include/fcl/narrowphase/contact.h | 4 +- .../continuous_collision_request.h | 4 +- .../narrowphase/continuous_collision_result.h | 4 +- .../detail/convexity_based_algorithm/epa.h | 6 +- .../detail/convexity_based_algorithm/gjk.h | 4 +- .../minkowski_diff.h | 4 +- .../detail/failed_at_this_configuration.h | 2 +- .../triangle_distance-inl.h | 934 +++---- .../triangle_distance.h | 228 +- .../collision/bvh_collision_traversal_node.h | 2 +- .../collision/collision_traversal_node_base.h | 2 +- .../traversal/collision/intersect-inl.h | 2292 ++++++++--------- .../detail/traversal/collision/intersect.h | 530 ++-- .../collision/mesh_collision_traversal_node.h | 4 +- .../mesh_shape_collision_traversal_node.h | 2 +- ..._conservative_advancement_traversal_node.h | 4 +- .../distance/mesh_distance_traversal_node.h | 2 +- ..._conservative_advancement_traversal_node.h | 2 +- .../mesh_shape_distance_traversal_node.h | 6 +- .../shape_bvh_distance_traversal_node.h | 2 +- ..._conservative_advancement_traversal_node.h | 2 +- .../shape_mesh_distance_traversal_node.h | 8 +- .../detail/traversal/traversal_node_base.h | 4 +- include/fcl/narrowphase/distance.h | 2 +- include/fcl/narrowphase/distance_result.h | 2 +- src/broadphase/broadphase_SSaP.cpp | 2 +- src/broadphase/broadphase_SaP.cpp | 2 +- src/broadphase/broadphase_bruteforce.cpp | 2 +- .../broadphase_collision_manager.cpp | 2 +- ...roadphase_continuous_collision_manager.cpp | 2 +- .../broadphase_dynamic_AABB_tree.cpp | 2 +- .../broadphase_dynamic_AABB_tree_array.cpp | 2 +- src/broadphase/broadphase_interval_tree.cpp | 2 +- src/broadphase/broadphase_spatialhash.cpp | 2 +- src/broadphase/detail/spatial_hash.cpp | 2 +- src/geometry/octree/octree.cpp | 2 +- src/math/bv/kIOS.cpp | 2 +- src/math/detail/polysolver.cpp | 98 +- src/math/detail/project.cpp | 100 +- .../primitive_shape_algorithm/intersect.cpp | 100 +- .../triangle_distance.cpp | 102 +- 91 files changed, 3080 insertions(+), 3080 deletions(-) diff --git a/include/fcl/broadphase/broadphase_SSaP-inl.h b/include/fcl/broadphase/broadphase_SSaP-inl.h index 526b620c0..4a3b67568 100644 --- a/include/fcl/broadphase/broadphase_SSaP-inl.h +++ b/include/fcl/broadphase/broadphase_SSaP-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/include/fcl/broadphase/broadphase_SSaP.h b/include/fcl/broadphase/broadphase_SSaP.h index 1d0225e76..185b9a0c2 100644 --- a/include/fcl/broadphase/broadphase_SSaP.h +++ b/include/fcl/broadphase/broadphase_SSaP.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -44,7 +44,7 @@ namespace fcl { -/// @brief Simple SAP collision manager +/// @brief Simple SAP collision manager template class SSaPCollisionManager : public BroadPhaseCollisionManager { @@ -89,7 +89,7 @@ class SSaPCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; @@ -97,13 +97,13 @@ class SSaPCollisionManager : public BroadPhaseCollisionManager /// @brief check collision between one object and a list of objects, return value is whether stop is possible bool checkColl(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const; - + /// @brief check distance between one object and a list of objects, return value is whether stop is possible bool checkDis(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; - + bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; static size_t selectOptimalAxis( diff --git a/include/fcl/broadphase/broadphase_SaP-inl.h b/include/fcl/broadphase/broadphase_SaP-inl.h index a4ea8cac4..3bfaa9588 100644 --- a/include/fcl/broadphase/broadphase_SaP-inl.h +++ b/include/fcl/broadphase/broadphase_SaP-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/include/fcl/broadphase/broadphase_SaP.h b/include/fcl/broadphase/broadphase_SaP.h index e5b210c27..869e6a26b 100644 --- a/include/fcl/broadphase/broadphase_SaP.h +++ b/include/fcl/broadphase/broadphase_SaP.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -103,7 +103,7 @@ class SaPCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; @@ -130,7 +130,7 @@ class SaPCollisionManager : public BroadPhaseCollisionManager /// @brief End point list for x, y, z coordinates EndPoint* elist[3]; - + /// @brief vector version of elist, for acceleration std::vector velist[3]; diff --git a/include/fcl/broadphase/broadphase_bruteforce-inl.h b/include/fcl/broadphase/broadphase_bruteforce-inl.h index 1958e9748..89aa74816 100644 --- a/include/fcl/broadphase/broadphase_bruteforce-inl.h +++ b/include/fcl/broadphase/broadphase_bruteforce-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/include/fcl/broadphase/broadphase_bruteforce.h b/include/fcl/broadphase/broadphase_bruteforce.h index 322576b02..2f9179dea 100644 --- a/include/fcl/broadphase/broadphase_bruteforce.h +++ b/include/fcl/broadphase/broadphase_bruteforce.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -92,7 +92,7 @@ class NaiveCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_collision_manager.h b/include/fcl/broadphase/broadphase_collision_manager.h index 6c4306a69..c60c21569 100644 --- a/include/fcl/broadphase/broadphase_collision_manager.h +++ b/include/fcl/broadphase/broadphase_collision_manager.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -117,7 +117,7 @@ class BroadPhaseCollisionManager /// @brief whether the manager is empty virtual bool empty() const = 0; - + /// @brief the number of objects managed by the manager virtual size_t size() const = 0; diff --git a/include/fcl/broadphase/broadphase_continuous_collision_manager.h b/include/fcl/broadphase/broadphase_continuous_collision_manager.h index f9950eb46..7cfa2156c 100644 --- a/include/fcl/broadphase/broadphase_continuous_collision_manager.h +++ b/include/fcl/broadphase/broadphase_continuous_collision_manager.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -117,7 +117,7 @@ class BroadPhaseContinuousCollisionManager /// @brief whether the manager is empty virtual bool empty() const = 0; - + /// @brief the number of objects managed by the manager virtual size_t size() const = 0; }; diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h index f792f563b..7f3defa89 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h index e6cfc8902..bfe0ef6db 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -71,7 +71,7 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager /// @brief add objects to the manager void registerObjects(const std::vector*>& other_objs); - + /// @brief add one object to the manager void registerObject(CollisionObject* obj); @@ -113,10 +113,10 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; - + /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index 1080a42cc..845579835 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h index 58b1432b9..df4421edb 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -67,12 +67,12 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager< bool octree_as_geometry_collide; bool octree_as_geometry_distance; - + DynamicAABBTreeCollisionManager_Array(); /// @brief add objects to the manager void registerObjects(const std::vector*>& other_objs); - + /// @brief add one object to the manager void registerObject(CollisionObject* obj); @@ -114,10 +114,10 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager< /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; - + /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_interval_tree-inl.h b/include/fcl/broadphase/broadphase_interval_tree-inl.h index f7f6f95e9..f959a1982 100644 --- a/include/fcl/broadphase/broadphase_interval_tree-inl.h +++ b/include/fcl/broadphase/broadphase_interval_tree-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/include/fcl/broadphase/broadphase_interval_tree.h b/include/fcl/broadphase/broadphase_interval_tree.h index b7b174d0c..8d28f2143 100644 --- a/include/fcl/broadphase/broadphase_interval_tree.h +++ b/include/fcl/broadphase/broadphase_interval_tree.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -99,7 +99,7 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_spatialhash-inl.h b/include/fcl/broadphase/broadphase_spatialhash-inl.h index 85c3a6a0f..3bb86f02a 100644 --- a/include/fcl/broadphase/broadphase_spatialhash-inl.h +++ b/include/fcl/broadphase/broadphase_spatialhash-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/include/fcl/broadphase/broadphase_spatialhash.h b/include/fcl/broadphase/broadphase_spatialhash.h index 0cedef753..96529b2f4 100644 --- a/include/fcl/broadphase/broadphase_spatialhash.h +++ b/include/fcl/broadphase/broadphase_spatialhash.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/include/fcl/broadphase/detail/hierarchy_tree.h b/include/fcl/broadphase/detail/hierarchy_tree.h index bdabe905b..bdea6c940 100644 --- a/include/fcl/broadphase/detail/hierarchy_tree.h +++ b/include/fcl/broadphase/detail/hierarchy_tree.h @@ -70,7 +70,7 @@ class HierarchyTree HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); - + /// @brief Initialize the tree by a set of leaves using algorithm with a given level. void init(std::vector& leaves, int level = 0); @@ -80,10 +80,10 @@ class HierarchyTree /// @brief Remove a leaf node void remove(NodeType* leaf); - /// @brief Clear the tree + /// @brief Clear the tree void clear(); - /// @brief Whether the tree is empty + /// @brief Whether the tree is empty bool empty() const; /// @brief Updates a `leaf` node. A use case is when the bounding volume @@ -102,10 +102,10 @@ class HierarchyTree /// @brief update the tree when the bounding volume of a given leaf has changed bool update(NodeType* leaf, const BV& bv); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(NodeType* leaf, const BV& bv, const Vector3& vel, S margin); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(NodeType* leaf, const BV& bv, const Vector3& vel); /// @brief get the max height of the tree @@ -114,19 +114,19 @@ class HierarchyTree /// @brief get the max depth of the tree size_t getMaxDepth() const; - /// @brief balance the tree from bottom + /// @brief balance the tree from bottom void balanceBottomup(); - /// @brief balance the tree from top + /// @brief balance the tree from top void balanceTopdown(); - - /// @brief balance the tree in an incremental way + + /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); - + /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner void refit(); - /// @brief extract all the leaves of the tree + /// @brief extract all the leaves of the tree void extractLeaves(const NodeType* root, std::vector& leaves) const; /// @brief number of leaves in the tree @@ -153,10 +153,10 @@ class HierarchyTree } }; - /// @brief construct a tree for a set of leaves from bottom -- very heavy way + /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief construct a tree for a set of leaves from top + /// @brief construct a tree for a set of leaves from top NodeType* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief compute the maximum height of a subtree rooted from a given node @@ -184,24 +184,24 @@ class HierarchyTree void init_1(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the node index. + /// we split the leaves into two parts with the same size simply using the node index. void init_2(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. void init_3(std::vector& leaves); - + NodeType* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32& split, int bits); NodeType* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32& split, int bits); NodeType* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief update one leaf node's bounding volume + /// @brief update one leaf node's bounding volume void update_(NodeType* leaf, const BV& bv); - /// @brief sort node n and its parent according to their memory position + /// @brief sort node n and its parent according to their memory position NodeType* sort(NodeType* n, NodeType*& r); - + /// @brief Insert a leaf node and also update its ancestors. Maintain the /// tree as a full binary tree (every interior node has exactly two children). /// Furthermore, adjust the BV of interior nodes so that each parent's BV @@ -219,13 +219,13 @@ class HierarchyTree // adjusted. NodeType* removeLeaf(NodeType* const leaf); - /// @brief Delete all internal nodes and return all leaves nodes with given depth from root + /// @brief Delete all internal nodes and return all leaves nodes with given depth from root void fetchLeaves(NodeType* root, std::vector& leaves, int depth = -1); static size_t indexOf(NodeType* node); - /// @brief create one node (leaf or internal) - NodeType* createNode(NodeType* parent, + /// @brief create one node (leaf or internal) + NodeType* createNode(NodeType* parent, const BV& bv, void* data); @@ -233,7 +233,7 @@ class HierarchyTree const BV& bv1, const BV& bv2, void* data); - + NodeType* createNode(NodeType* parent, void* data); @@ -250,11 +250,11 @@ class HierarchyTree unsigned int opath; - /// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently. - NodeType* free_node; + /// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently. + NodeType* free_node; int max_lookahead_level; - + public: /// @brief decide which topdown algorithm to use int topdown_level; diff --git a/include/fcl/broadphase/detail/hierarchy_tree_array.h b/include/fcl/broadphase/detail/hierarchy_tree_array.h index bd5e40328..8145a04be 100644 --- a/include/fcl/broadphase/detail/hierarchy_tree_array.h +++ b/include/fcl/broadphase/detail/hierarchy_tree_array.h @@ -62,7 +62,7 @@ class HierarchyTree { using S = typename BV::S; typedef NodeBase NodeType; - + struct SortByMorton { SortByMorton(NodeType* nodes_in) : nodes(nodes_in) {} @@ -102,22 +102,22 @@ class HierarchyTree /// @brief Remove a leaf node void remove(size_t leaf); - /// @brief Clear the tree + /// @brief Clear the tree void clear(); - /// @brief Whether the tree is empty + /// @brief Whether the tree is empty bool empty() const; - - /// @brief update one leaf node + + /// @brief update one leaf node void update(size_t leaf, int lookahead_level = -1); /// @brief update the tree when the bounding volume of a given leaf has changed bool update(size_t leaf, const BV& bv); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vector3& vel, S margin); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vector3& vel); /// @brief get the max height of the tree @@ -126,19 +126,19 @@ class HierarchyTree /// @brief get the max depth of the tree size_t getMaxDepth() const; - /// @brief balance the tree from bottom + /// @brief balance the tree from bottom void balanceBottomup(); - /// @brief balance the tree from top + /// @brief balance the tree from top void balanceTopdown(); - /// @brief balance the tree in an incremental way + /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner void refit(); - /// @brief extract all the leaves of the tree + /// @brief extract all the leaves of the tree void extractLeaves(size_t root, NodeType*& leaves) const; /// @brief number of leaves in the tree @@ -155,10 +155,10 @@ class HierarchyTree private: - /// @brief construct a tree for a set of leaves from bottom -- very heavy way + /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(size_t* lbeg, size_t* lend); - - /// @brief construct a tree for a set of leaves from top + + /// @brief construct a tree for a set of leaves from top size_t topdown(size_t* lbeg, size_t* lend); /// @brief compute the maximum height of a subtree rooted from a given node @@ -186,7 +186,7 @@ class HierarchyTree void init_1(NodeType* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the node index. + /// we split the leaves into two parts with the same size simply using the node index. void init_2(NodeType* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. @@ -198,17 +198,17 @@ class HierarchyTree size_t mortonRecurse_2(size_t* lbeg, size_t* lend); - /// @brief update one leaf node's bounding volume + /// @brief update one leaf node's bounding volume void update_(size_t leaf, const BV& bv); - /// @brief Insert a leaf node and also update its ancestors + /// @brief Insert a leaf node and also update its ancestors void insertLeaf(size_t root, size_t leaf); /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted. - /// return the node with the smallest depth and is influenced by the remove operation + /// return the node with the smallest depth and is influenced by the remove operation size_t removeLeaf(size_t leaf); - /// @brief Delete all internal nodes and return all leaves nodes with given depth from root + /// @brief Delete all internal nodes and return all leaves nodes with given depth from root void fetchLeaves(size_t root, NodeType*& leaves, int depth = -1); size_t indexOf(size_t node); @@ -216,13 +216,13 @@ class HierarchyTree size_t allocateNode(); /// @brief create one node (leaf or internal) - size_t createNode(size_t parent, + size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data); size_t createNode(size_t parent, - const BV& bv, + const BV& bv, void* data); size_t createNode(size_t parent, @@ -237,7 +237,7 @@ class HierarchyTree NodeType* nodes; size_t n_nodes; size_t n_nodes_alloc; - + size_t n_leaves; size_t freelist; unsigned int opath; diff --git a/include/fcl/broadphase/detail/interval_tree.h b/include/fcl/broadphase/detail/interval_tree.h index a2a2f317f..714dac7a8 100644 --- a/include/fcl/broadphase/detail/interval_tree.h +++ b/include/fcl/broadphase/detail/interval_tree.h @@ -113,7 +113,7 @@ class IntervalTree /// @brief Inserts node into the tree as if it were a regular binary tree void recursiveInsert(IntervalTreeNode* node); - /// @brief recursively print a subtree + /// @brief recursively print a subtree void recursivePrint(IntervalTreeNode* node) const; /// @brief recursively find the node corresponding to the interval diff --git a/include/fcl/broadphase/detail/interval_tree_node.h b/include/fcl/broadphase/detail/interval_tree_node.h index 9d60e6e8d..8e243b3e3 100644 --- a/include/fcl/broadphase/detail/interval_tree_node.h +++ b/include/fcl/broadphase/detail/interval_tree_node.h @@ -60,7 +60,7 @@ class IntervalTreeNode friend class IntervalTree; friend class IntervalTree; - + /// @brief Create an empty node IntervalTreeNode(); @@ -83,7 +83,7 @@ class IntervalTreeNode S max_high; /// @brief red or black node: if red = false then the node is black - bool red; + bool red; IntervalTreeNode* left; diff --git a/include/fcl/broadphase/detail/simple_interval.h b/include/fcl/broadphase/detail/simple_interval.h index 0c5e8e253..1b8516381 100644 --- a/include/fcl/broadphase/detail/simple_interval.h +++ b/include/fcl/broadphase/detail/simple_interval.h @@ -53,7 +53,7 @@ struct SimpleInterval { public: virtual ~SimpleInterval(); - + virtual void print(); /// @brief interval is defined as [low, high] diff --git a/include/fcl/broadphase/detail/sparse_hash_table.h b/include/fcl/broadphase/detail/sparse_hash_table.h index 7d1f07aef..f2d4f0f4f 100644 --- a/include/fcl/broadphase/detail/sparse_hash_table.h +++ b/include/fcl/broadphase/detail/sparse_hash_table.h @@ -62,14 +62,14 @@ class SparseHashTable HashFnc h_; typedef std::list Bin; typedef TableT Table; - + Table table_; public: SparseHashTable(const HashFnc& h); /// @brief Init the hash table. The bucket size is dynamically decided. void init(size_t); - + /// @brief insert one key-value pair into the hash table void insert(Key key, Data value); diff --git a/include/fcl/broadphase/detail/spatial_hash-inl.h b/include/fcl/broadphase/detail/spatial_hash-inl.h index 9eba06d63..b97354f4b 100644 --- a/include/fcl/broadphase/detail/spatial_hash-inl.h +++ b/include/fcl/broadphase/detail/spatial_hash-inl.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/include/fcl/broadphase/detail/spatial_hash.h b/include/fcl/broadphase/detail/spatial_hash.h index f3fdf7e15..ebb25a57b 100644 --- a/include/fcl/broadphase/detail/spatial_hash.h +++ b/include/fcl/broadphase/detail/spatial_hash.h @@ -31,7 +31,7 @@ * 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 Jia Pan */ @@ -53,7 +53,7 @@ struct SpatialHash using S = S_; SpatialHash(const AABB& scene_limit_, S cell_size_); - + std::vector operator() (const AABB& aabb) const; private: diff --git a/include/fcl/geometry/bvh/BVH_model.h b/include/fcl/geometry/bvh/BVH_model.h index 15849ee24..bab7304f5 100644 --- a/include/fcl/geometry/bvh/BVH_model.h +++ b/include/fcl/geometry/bvh/BVH_model.h @@ -74,7 +74,7 @@ class BVHModel : public CollisionGeometry /// @brief We provide getBV() and getNumBVs() because BVH may be compressed /// (in future), so we must provide some flexibility here - + /// @brief Access the bv giving the its index const BVNode& getBV(int id) const; @@ -147,7 +147,7 @@ class BVHModel : public CollisionGeometry /// @brief Check the number of memory used int memUsage(int msg) const; - /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent + /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent /// BV node. When traversing the BVH, this can save one matrix transformation. void makeParentRelative(); @@ -211,7 +211,7 @@ class BVHModel : public CollisionGeometry /// @brief Recursive kernel for hierarchy construction int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives); - /// @brief Recursive kernel for bottomup refitting + /// @brief Recursive kernel for bottomup refitting int recursiveRefitTree_bottomup(int bv_id); /// @recursively compute each bv's transform related to its parent. For diff --git a/include/fcl/geometry/bvh/BV_node_base.h b/include/fcl/geometry/bvh/BV_node_base.h index 2b7f175a0..63d9e4be9 100644 --- a/include/fcl/geometry/bvh/BV_node_base.h +++ b/include/fcl/geometry/bvh/BV_node_base.h @@ -59,7 +59,7 @@ struct FCL_API BVNodeBase /// we can obtain the primitive's index in original data indirectly. int first_primitive; - /// @brief The number of primitives belonging to the current node + /// @brief The number of primitives belonging to the current node int num_primitives; /// @brief Whether current node is a leaf node (i.e. contains a primitive index diff --git a/include/fcl/geometry/octree/octree.h b/include/fcl/geometry/octree/octree.h index b23b09dd8..d0a6b1c22 100644 --- a/include/fcl/geometry/octree/octree.h +++ b/include/fcl/geometry/octree/octree.h @@ -125,7 +125,7 @@ class OcTree : public CollisionGeometry /// @return const ptr to child number childIdx of node const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const; - + /// @brief return true if the child at childIdx exists bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const; diff --git a/include/fcl/geometry/shape/capsule.h b/include/fcl/geometry/shape/capsule.h index 8c21a96fe..831deed8c 100644 --- a/include/fcl/geometry/shape/capsule.h +++ b/include/fcl/geometry/shape/capsule.h @@ -45,7 +45,7 @@ namespace fcl { -/// @brief Center at zero point capsule +/// @brief Center at zero point capsule template class Capsule : public ShapeBase { @@ -56,16 +56,16 @@ class Capsule : public ShapeBase /// @brief Constructor Capsule(S radius, S lz); - /// @brief Radius of capsule + /// @brief Radius of capsule S radius; - /// @brief Length along z axis + /// @brief Length along z axis S lz; /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a capsule + /// @brief Get node type: a capsule NODE_TYPE getNodeType() const override; // Documentation inherited diff --git a/include/fcl/geometry/shape/cone.h b/include/fcl/geometry/shape/cone.h index 30fff4305..528122069 100644 --- a/include/fcl/geometry/shape/cone.h +++ b/include/fcl/geometry/shape/cone.h @@ -45,7 +45,7 @@ namespace fcl { -/// @brief Center at zero cone +/// @brief Center at zero cone template class Cone : public ShapeBase { @@ -55,16 +55,16 @@ class Cone : public ShapeBase Cone(S radius, S lz); - /// @brief Radius of the cone + /// @brief Radius of the cone S radius; - /// @brief Length along z axis + /// @brief Length along z axis S lz; /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a cone + /// @brief Get node type: a cone NODE_TYPE getNodeType() const override; // Documentation inherited diff --git a/include/fcl/geometry/shape/convex.h b/include/fcl/geometry/shape/convex.h index e05f50aea..e911613c8 100644 --- a/include/fcl/geometry/shape/convex.h +++ b/include/fcl/geometry/shape/convex.h @@ -110,7 +110,7 @@ class Convex : public ShapeBase int num_faces, const std::shared_ptr>& faces, bool throw_if_invalid = false); - /// @brief Copy constructor + /// @brief Copy constructor Convex(const Convex& other); ~Convex() = default; diff --git a/include/fcl/geometry/shape/cylinder.h b/include/fcl/geometry/shape/cylinder.h index 6e7280d1e..5e33aecfd 100644 --- a/include/fcl/geometry/shape/cylinder.h +++ b/include/fcl/geometry/shape/cylinder.h @@ -45,7 +45,7 @@ namespace fcl { -/// @brief Center at zero cylinder +/// @brief Center at zero cylinder template class Cylinder : public ShapeBase { @@ -55,17 +55,17 @@ class Cylinder : public ShapeBase /// @brief Constructor Cylinder(S radius, S lz); - - /// @brief Radius of the cylinder + + /// @brief Radius of the cylinder S radius; - /// @brief Length along z axis + /// @brief Length along z axis S lz; - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a cylinder + /// @brief Get node type: a cylinder NODE_TYPE getNodeType() const override; // Documentation inherited diff --git a/include/fcl/geometry/shape/halfspace.h b/include/fcl/geometry/shape/halfspace.h index 7d14c14c4..641241cf0 100644 --- a/include/fcl/geometry/shape/halfspace.h +++ b/include/fcl/geometry/shape/halfspace.h @@ -79,10 +79,10 @@ class Halfspace : public ShapeBase /// @brief Get node type: a half space NODE_TYPE getNodeType() const override; - + /// @brief Planed normal Vector3 n; - + /// @brief Planed offset S d; diff --git a/include/fcl/geometry/shape/plane.h b/include/fcl/geometry/shape/plane.h index 77a8ce2f7..6dfb07458 100644 --- a/include/fcl/geometry/shape/plane.h +++ b/include/fcl/geometry/shape/plane.h @@ -45,7 +45,7 @@ namespace fcl { -/// @brief Infinite plane +/// @brief Infinite plane template class Plane : public ShapeBase { @@ -53,10 +53,10 @@ class Plane : public ShapeBase using S = S_; - /// @brief Construct a plane with normal direction and offset + /// @brief Construct a plane with normal direction and offset Plane(const Vector3& n, S d); - - /// @brief Construct a plane with normal direction and offset + + /// @brief Construct a plane with normal direction and offset Plane(S a, S b, S c, S d); Plane(); @@ -68,13 +68,13 @@ class Plane : public ShapeBase /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a plane + /// @brief Get node type: a plane NODE_TYPE getNodeType() const override; - /// @brief Plane normal + /// @brief Plane normal Vector3 n; - /// @brief Plane offset + /// @brief Plane offset S d; friend @@ -84,8 +84,8 @@ class Plane : public ShapeBase } protected: - - /// @brief Turn non-unit normal into unit + + /// @brief Turn non-unit normal into unit void unitNormalTest(); }; diff --git a/include/fcl/geometry/shape/triangle_p.h b/include/fcl/geometry/shape/triangle_p.h index da3c92a53..28e911ac0 100644 --- a/include/fcl/geometry/shape/triangle_p.h +++ b/include/fcl/geometry/shape/triangle_p.h @@ -59,7 +59,7 @@ class TriangleP : public ShapeBase /// @brief virtual function of compute AABB in local coordinate void computeLocalAABB() override; - + // Documentation inherited NODE_TYPE getNodeType() const override; diff --git a/include/fcl/math/bv/OBB.h b/include/fcl/math/bv/OBB.h index 1f3f0c4ef..c611070ed 100644 --- a/include/fcl/math/bv/OBB.h +++ b/include/fcl/math/bv/OBB.h @@ -75,10 +75,10 @@ class OBB const Vector3& center, const Vector3& extent); - /// @brief Check collision between two OBB, return true if collision happens. + /// @brief Check collision between two OBB, return true if collision happens. bool overlap(const OBB& other) const; - - /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. + + /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. bool overlap(const OBB& other, OBB& overlap_part) const; /// @brief Check whether the OBB contains a point. diff --git a/include/fcl/math/bv/kDOP.h b/include/fcl/math/bv/kDOP.h index 31dc4ba74..8b312162c 100644 --- a/include/fcl/math/bv/kDOP.h +++ b/include/fcl/math/bv/kDOP.h @@ -47,7 +47,7 @@ namespace fcl { /// @brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 -/// The KDOP structure is defined by some pairs of parallel planes defined by some axes. +/// The KDOP structure is defined by some pairs of parallel planes defined by some axes. /// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 8 /// (0,-1,0) and (0,1,0) -> indices 1 and 9 @@ -95,7 +95,7 @@ class KDOP /// @brief Creating kDOP containing two points KDOP(const Vector3& a, const Vector3& b); - + /// @brief Check whether two KDOPs are overlapped bool overlap(const KDOP& other) const; diff --git a/include/fcl/math/bv/kIOS.h b/include/fcl/math/bv/kIOS.h index 0ce7ba496..3cf941778 100644 --- a/include/fcl/math/bv/kIOS.h +++ b/include/fcl/math/bv/kIOS.h @@ -42,7 +42,7 @@ namespace fcl { - + /// @brief A class describing the kIOS collision structure, which is a set of spheres. template class kIOS diff --git a/include/fcl/math/detail/polysolver-inl.h b/include/fcl/math/detail/polysolver-inl.h index 24370c528..405fea6e3 100644 --- a/include/fcl/math/detail/polysolver-inl.h +++ b/include/fcl/math/detail/polysolver-inl.h @@ -1,201 +1,201 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H -#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H - -#include "fcl/math/detail/polysolver.h" - -#include -#include "fcl/common/types.h" - -namespace fcl -{ - -namespace detail { - -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API PolySolver; - -//============================================================================== -template -int PolySolver::solveLinear(S c[2], S s[1]) -{ - if(isZero(c[1])) - return 0; - s[0] = - c[0] / c[1]; - return 1; -} - -//============================================================================== -template -int PolySolver::solveQuadric(S c[3], S s[2]) -{ - S p, q, D; - - // make sure we have a d2 equation - - if(isZero(c[2])) - return solveLinear(c, s); - - // normal for: x^2 + px + q - p = c[1] / (2.0 * c[2]); - q = c[0] / c[2]; - D = p * p - q; - - if(isZero(D)) - { - // one S root - s[0] = s[1] = -p; - return 1; - } - - if(D < 0.0) - // no real root - return 0; - else - { - // two real roots - S sqrt_D = sqrt(D); - s[0] = sqrt_D - p; - s[1] = -sqrt_D - p; - return 2; - } -} - -//============================================================================== -template -int PolySolver::solveCubic(S c[4], S s[3]) -{ - int i, num; - S sub, A, B, C, sq_A, p, q, cb_p, D; - const S ONE_OVER_THREE = 1 / 3.0; - const S PI = 3.14159265358979323846; - - // make sure we have a d2 equation - if(isZero(c[3])) - return solveQuadric(c, s); - - // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 - A = c[2] / c[3]; - B = c[1] / c[3]; - C = c[0] / c[3]; - - // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 - sq_A = A * A; - p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; - q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); - - // use Cardano's formula - cb_p = p * p * p; - D = q * q + cb_p; - - if(isZero(D)) - { - if(isZero(q)) - { - // one triple solution - s[0] = 0.0; - num = 1; - } - else - { - // one single and one S solution - S u = cbrt(-q); - s[0] = 2.0 * u; - s[1] = -u; - num = 2; - } - } - else - { - if(D < 0.0) - { - // three real solutions - S phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); - S t = 2.0 * sqrt(-p); - s[0] = t * cos(phi); - s[1] = -t * cos(phi + PI / 3.0); - s[2] = -t * cos(phi - PI / 3.0); - num = 3; - } - else - { - // one real solution - S sqrt_D = sqrt(D); - S u = cbrt(sqrt_D + fabs(q)); - if(q > 0.0) - s[0] = - u + p / u ; - else - s[0] = u - p / u; - num = 1; - } - } - - // re-substitute - sub = ONE_OVER_THREE * A; - for(i = 0; i < num; i++) - s[i] -= sub; - return num; -} - -//============================================================================== -template -bool PolySolver::isZero(S v) -{ - return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); -} - -//============================================================================== -template -bool PolySolver::cbrt(S v) -{ - return std::pow(v, 1.0 / 3.0); -} - -//============================================================================== -template -constexpr S PolySolver::getNearZeroThreshold() -{ - return 1e-9; -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H +#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H + +#include "fcl/math/detail/polysolver.h" + +#include +#include "fcl/common/types.h" + +namespace fcl +{ + +namespace detail { + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API PolySolver; + +//============================================================================== +template +int PolySolver::solveLinear(S c[2], S s[1]) +{ + if(isZero(c[1])) + return 0; + s[0] = - c[0] / c[1]; + return 1; +} + +//============================================================================== +template +int PolySolver::solveQuadric(S c[3], S s[2]) +{ + S p, q, D; + + // make sure we have a d2 equation + + if(isZero(c[2])) + return solveLinear(c, s); + + // normal for: x^2 + px + q + p = c[1] / (2.0 * c[2]); + q = c[0] / c[2]; + D = p * p - q; + + if(isZero(D)) + { + // one S root + s[0] = s[1] = -p; + return 1; + } + + if(D < 0.0) + // no real root + return 0; + else + { + // two real roots + S sqrt_D = sqrt(D); + s[0] = sqrt_D - p; + s[1] = -sqrt_D - p; + return 2; + } +} + +//============================================================================== +template +int PolySolver::solveCubic(S c[4], S s[3]) +{ + int i, num; + S sub, A, B, C, sq_A, p, q, cb_p, D; + const S ONE_OVER_THREE = 1 / 3.0; + const S PI = 3.14159265358979323846; + + // make sure we have a d2 equation + if(isZero(c[3])) + return solveQuadric(c, s); + + // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 + A = c[2] / c[3]; + B = c[1] / c[3]; + C = c[0] / c[3]; + + // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 + sq_A = A * A; + p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; + q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); + + // use Cardano's formula + cb_p = p * p * p; + D = q * q + cb_p; + + if(isZero(D)) + { + if(isZero(q)) + { + // one triple solution + s[0] = 0.0; + num = 1; + } + else + { + // one single and one S solution + S u = cbrt(-q); + s[0] = 2.0 * u; + s[1] = -u; + num = 2; + } + } + else + { + if(D < 0.0) + { + // three real solutions + S phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); + S t = 2.0 * sqrt(-p); + s[0] = t * cos(phi); + s[1] = -t * cos(phi + PI / 3.0); + s[2] = -t * cos(phi - PI / 3.0); + num = 3; + } + else + { + // one real solution + S sqrt_D = sqrt(D); + S u = cbrt(sqrt_D + fabs(q)); + if(q > 0.0) + s[0] = - u + p / u ; + else + s[0] = u - p / u; + num = 1; + } + } + + // re-substitute + sub = ONE_OVER_THREE * A; + for(i = 0; i < num; i++) + s[i] -= sub; + return num; +} + +//============================================================================== +template +bool PolySolver::isZero(S v) +{ + return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); +} + +//============================================================================== +template +bool PolySolver::cbrt(S v) +{ + return std::pow(v, 1.0 / 3.0); +} + +//============================================================================== +template +constexpr S PolySolver::getNearZeroThreshold() +{ + return 1e-9; +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/math/detail/polysolver.h b/include/fcl/math/detail/polysolver.h index d79217f2d..bcad09f86 100644 --- a/include/fcl/math/detail/polysolver.h +++ b/include/fcl/math/detail/polysolver.h @@ -1,80 +1,80 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_H -#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_H - -#include "fcl/export.h" - -namespace fcl -{ - -namespace detail { - -/// @brief A class solves polynomial degree (1,2,3) equations -template -class PolySolver -{ -public: - /// @brief Solve a linear equation with coefficients c, return roots s and number of roots - static int solveLinear(S c[2], S s[1]); - - /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots - static int solveQuadric(S c[3], S s[2]); - - /// @brief Solve a cubic function with coefficients c, return roots s and number of roots - static int solveCubic(S c[4], S s[3]); - -private: - /// @brief Check whether v is zero - static bool isZero(S v); - - /// @brief Compute v^{1/3} - static bool cbrt(S v); - - static constexpr S getNearZeroThreshold(); -}; - -using PolySolverf = PolySolver; -using PolySolverd = PolySolver; - -} // namespace detail -} // namespace fcl - -#include "fcl/math/detail/polysolver-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_H +#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_H + +#include "fcl/export.h" + +namespace fcl +{ + +namespace detail { + +/// @brief A class solves polynomial degree (1,2,3) equations +template +class PolySolver +{ +public: + /// @brief Solve a linear equation with coefficients c, return roots s and number of roots + static int solveLinear(S c[2], S s[1]); + + /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots + static int solveQuadric(S c[3], S s[2]); + + /// @brief Solve a cubic function with coefficients c, return roots s and number of roots + static int solveCubic(S c[4], S s[3]); + +private: + /// @brief Check whether v is zero + static bool isZero(S v); + + /// @brief Compute v^{1/3} + static bool cbrt(S v); + + static constexpr S getNearZeroThreshold(); +}; + +using PolySolverf = PolySolver; +using PolySolverd = PolySolver; + +} // namespace detail +} // namespace fcl + +#include "fcl/math/detail/polysolver-inl.h" + +#endif diff --git a/include/fcl/math/detail/project-inl.h b/include/fcl/math/detail/project-inl.h index 9cccf5e51..1b29dffc2 100644 --- a/include/fcl/math/detail/project-inl.h +++ b/include/fcl/math/detail/project-inl.h @@ -1,320 +1,320 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_INL_H -#define FCL_NARROWPHASE_DETAIL_PROJECT_INL_H - -#include "fcl/math/detail/project.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API Project; - -//============================================================================== -template -typename Project::ProjectResult Project::projectLine(const Vector3& a, const Vector3& b, const Vector3& p) -{ - ProjectResult res; - - const Vector3 d = b - a; - const S l = d.squaredNorm(); - - if(l > 0) - { - const S t = (p - a).dot(d); - res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); - res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } - } - - return res; -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p) -{ - ProjectResult res; - - static const size_t nexti[3] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c}; - const Vector3 dl[] = {a - b, b - c, c - a}; - const Vector3& n = dl[0].cross(dl[1]); - const S l = n.squaredNorm(); - - if(l > 0) - { - S mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge - { - size_t j = nexti[i]; - ProjectResult res_line = projectLine(*vt[i], *vt[j], p); - - if(mindist < 0 || res_line.sqr_distance < mindist) - { - mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1< p_to_project = n * (d / l); - mindist = p_to_project.squaredNorm(); - res.encode = 7; // m = 0x111 - res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s; - res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s; - res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; - } - - res.sqr_distance = mindist; - } - - return res; - -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p) -{ - ProjectResult res; - - static const size_t nexti[] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c, &d}; - const Vector3 dl[3] = {a-d, b-d, c-d}; - S vl = triple(dl[0], dl[1], dl[2]); - bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0; - if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) - { - S mindist = -1; - - for(size_t i = 0; i < 3; ++i) - { - size_t j = nexti[i]; - S s = vl * (d-p).dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face - { - ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { - mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1< -typename Project::ProjectResult Project::projectLineOrigin(const Vector3& a, const Vector3& b) -{ - ProjectResult res; - - const Vector3 d = b - a; - const S l = d.squaredNorm(); - - if(l > 0) - { - const S t = - a.dot(d); - res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); - res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } - } - - return res; -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c) -{ - ProjectResult res; - - static const size_t nexti[3] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c}; - const Vector3 dl[] = {a - b, b - c, c - a}; - const Vector3& n = dl[0].cross(dl[1]); - const S l = n.squaredNorm(); - - if(l > 0) - { - S mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge - { - size_t j = nexti[i]; - ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); - - if(mindist < 0 || res_line.sqr_distance < mindist) - { - mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1< o_to_project = n * (d / l); - mindist = o_to_project.squaredNorm(); - res.encode = 7; // m = 0x111 - res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; - res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; - res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; - } - - res.sqr_distance = mindist; - } - - return res; - -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) -{ - ProjectResult res; - - static const size_t nexti[] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c, &d}; - const Vector3 dl[3] = {a-d, b-d, c-d}; - S vl = triple(dl[0], dl[1], dl[2]); - bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0; - if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) - { - S mindist = -1; - - for(size_t i = 0; i < 3; ++i) - { - size_t j = nexti[i]; - S s = vl * d.dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face - { - ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { - mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1< -Project::ProjectResult::ProjectResult() - : parameterization{0.0, 0.0, 0.0, 0.0}, sqr_distance(-1), encode(0) -{ - // Do nothing -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_INL_H +#define FCL_NARROWPHASE_DETAIL_PROJECT_INL_H + +#include "fcl/math/detail/project.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API Project; + +//============================================================================== +template +typename Project::ProjectResult Project::projectLine(const Vector3& a, const Vector3& b, const Vector3& p) +{ + ProjectResult res; + + const Vector3 d = b - a; + const S l = d.squaredNorm(); + + if(l > 0) + { + const S t = (p - a).dot(d); + res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); + res.parameterization[0] = 1 - res.parameterization[1]; + if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } + else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } + else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } + } + + return res; +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p) +{ + ProjectResult res; + + static const size_t nexti[3] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c}; + const Vector3 dl[] = {a - b, b - c, c - a}; + const Vector3& n = dl[0].cross(dl[1]); + const S l = n.squaredNorm(); + + if(l > 0) + { + S mindist = -1; + for(size_t i = 0; i < 3; ++i) + { + if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + { + size_t j = nexti[i]; + ProjectResult res_line = projectLine(*vt[i], *vt[j], p); + + if(mindist < 0 || res_line.sqr_distance < mindist) + { + mindist = res_line.sqr_distance; + res.encode = static_cast(((res_line.encode&1)?1< p_to_project = n * (d / l); + mindist = p_to_project.squaredNorm(); + res.encode = 7; // m = 0x111 + res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s; + res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s; + res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + } + + res.sqr_distance = mindist; + } + + return res; + +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p) +{ + ProjectResult res; + + static const size_t nexti[] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c, &d}; + const Vector3 dl[3] = {a-d, b-d, c-d}; + S vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0; + if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + { + S mindist = -1; + + for(size_t i = 0; i < 3; ++i) + { + size_t j = nexti[i]; + S s = vl * (d-p).dot(dl[i].cross(dl[j])); + if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + { + ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); + if(mindist < 0 || res_triangle.sqr_distance < mindist) + { + mindist = res_triangle.sqr_distance; + res.encode = static_cast( (res_triangle.encode&1?1< +typename Project::ProjectResult Project::projectLineOrigin(const Vector3& a, const Vector3& b) +{ + ProjectResult res; + + const Vector3 d = b - a; + const S l = d.squaredNorm(); + + if(l > 0) + { + const S t = - a.dot(d); + res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); + res.parameterization[0] = 1 - res.parameterization[1]; + if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } + else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } + else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } + } + + return res; +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c) +{ + ProjectResult res; + + static const size_t nexti[3] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c}; + const Vector3 dl[] = {a - b, b - c, c - a}; + const Vector3& n = dl[0].cross(dl[1]); + const S l = n.squaredNorm(); + + if(l > 0) + { + S mindist = -1; + for(size_t i = 0; i < 3; ++i) + { + if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + { + size_t j = nexti[i]; + ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); + + if(mindist < 0 || res_line.sqr_distance < mindist) + { + mindist = res_line.sqr_distance; + res.encode = static_cast(((res_line.encode&1)?1< o_to_project = n * (d / l); + mindist = o_to_project.squaredNorm(); + res.encode = 7; // m = 0x111 + res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; + res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; + res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + } + + res.sqr_distance = mindist; + } + + return res; + +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) +{ + ProjectResult res; + + static const size_t nexti[] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c, &d}; + const Vector3 dl[3] = {a-d, b-d, c-d}; + S vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0; + if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + { + S mindist = -1; + + for(size_t i = 0; i < 3; ++i) + { + size_t j = nexti[i]; + S s = vl * d.dot(dl[i].cross(dl[j])); + if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + { + ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); + if(mindist < 0 || res_triangle.sqr_distance < mindist) + { + mindist = res_triangle.sqr_distance; + res.encode = static_cast( (res_triangle.encode&1?1< +Project::ProjectResult::ProjectResult() + : parameterization{0.0, 0.0, 0.0, 0.0}, sqr_distance(-1), encode(0) +{ + // Do nothing +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/math/detail/project.h b/include/fcl/math/detail/project.h index c08f065da..4b19b0dbe 100644 --- a/include/fcl/math/detail/project.h +++ b/include/fcl/math/detail/project.h @@ -1,96 +1,96 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_H -#define FCL_NARROWPHASE_DETAIL_PROJECT_H - -#include "fcl/common/types.h" -#include "fcl/math/geometry.h" - -namespace fcl -{ - -namespace detail -{ - -/// @brief Project functions -template -class Project -{ -public: - struct ProjectResult - { - /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array) - S parameterization[4]; - - /// @brief square distance from the query point to the projected simplex - S sqr_distance; - - /// @brief the code of the projection type - unsigned int encode; - - ProjectResult(); - }; - - /// @brief Project point p onto line a-b - static ProjectResult projectLine(const Vector3& a, const Vector3& b, const Vector3& p); - - /// @brief Project point p onto triangle a-b-c - static ProjectResult projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p); - - /// @brief Project point p onto tetrahedra a-b-c-d - static ProjectResult projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p); - - /// @brief Project origin (0) onto line a-b - static ProjectResult projectLineOrigin(const Vector3& a, const Vector3& b); - - /// @brief Project origin (0) onto triangle a-b-c - static ProjectResult projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c); - - /// @brief Project origin (0) onto tetrahedran a-b-c-d - static ProjectResult projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d); -}; - -using Projectf = Project; -using Projectd = Project; - -} // namespace detail -} // namespace fcl - -#include "fcl/math/detail/project-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_H +#define FCL_NARROWPHASE_DETAIL_PROJECT_H + +#include "fcl/common/types.h" +#include "fcl/math/geometry.h" + +namespace fcl +{ + +namespace detail +{ + +/// @brief Project functions +template +class Project +{ +public: + struct ProjectResult + { + /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array) + S parameterization[4]; + + /// @brief square distance from the query point to the projected simplex + S sqr_distance; + + /// @brief the code of the projection type + unsigned int encode; + + ProjectResult(); + }; + + /// @brief Project point p onto line a-b + static ProjectResult projectLine(const Vector3& a, const Vector3& b, const Vector3& p); + + /// @brief Project point p onto triangle a-b-c + static ProjectResult projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p); + + /// @brief Project point p onto tetrahedra a-b-c-d + static ProjectResult projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p); + + /// @brief Project origin (0) onto line a-b + static ProjectResult projectLineOrigin(const Vector3& a, const Vector3& b); + + /// @brief Project origin (0) onto triangle a-b-c + static ProjectResult projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c); + + /// @brief Project origin (0) onto tetrahedran a-b-c-d + static ProjectResult projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d); +}; + +using Projectf = Project; +using Projectd = Project; + +} // namespace detail +} // namespace fcl + +#include "fcl/math/detail/project-inl.h" + +#endif diff --git a/include/fcl/math/motion/interp_motion.h b/include/fcl/math/motion/interp_motion.h index 55ec717c9..69cbd8efb 100644 --- a/include/fcl/math/motion/interp_motion.h +++ b/include/fcl/math/motion/interp_motion.h @@ -81,7 +81,7 @@ class InterpMotion : public MotionBase /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const; - /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor + /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor S computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const; /// @brief Get the rotation and translation in current step @@ -94,9 +94,9 @@ class InterpMotion : public MotionBase void computeVelocity(); Quaternion deltaRotation(S dt) const; - + Quaternion absoluteRotation(S dt) const; - + /// @brief The transformation at time 0 Transform3 tf1; diff --git a/include/fcl/math/motion/spline_motion.h b/include/fcl/math/motion/spline_motion.h index 7267f4d5f..a6082ac04 100644 --- a/include/fcl/math/motion/spline_motion.h +++ b/include/fcl/math/motion/spline_motion.h @@ -63,7 +63,7 @@ class SplineMotion : public MotionBase SplineMotion(const Transform3& tf1, const Transform3& tf2); - + /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. bool integrate(S dt) const override; @@ -86,7 +86,7 @@ class SplineMotion : public MotionBase S getWeight1(S t) const; S getWeight2(S t) const; S getWeight3(S t) const; - + Vector3 Td[4]; Vector3 Rd[4]; @@ -102,7 +102,7 @@ class SplineMotion : public MotionBase public: S computeTBound(const Vector3& n) const; - + S computeDWMax() const; S getCurrentTime() const; diff --git a/include/fcl/math/motion/taylor_model/interval_vector.h b/include/fcl/math/motion/taylor_model/interval_vector.h index d4d3473f9..c59e81af2 100644 --- a/include/fcl/math/motion/taylor_model/interval_vector.h +++ b/include/fcl/math/motion/taylor_model/interval_vector.h @@ -72,7 +72,7 @@ struct IVector3 void setValue(const Vector3& v); void setValue(S v[3]); - + IVector3 operator + (const IVector3& other) const; IVector3& operator += (const IVector3& other); @@ -90,7 +90,7 @@ struct IVector3 Interval& operator [] (size_t i); Vector3 getLow() const; - + Vector3 getHigh() const; void print() const; diff --git a/include/fcl/math/motion/taylor_model/taylor_matrix.h b/include/fcl/math/motion/taylor_model/taylor_matrix.h index eb1180582..2c4a11126 100644 --- a/include/fcl/math/motion/taylor_model/taylor_matrix.h +++ b/include/fcl/math/motion/taylor_model/taylor_matrix.h @@ -48,7 +48,7 @@ template class TMatrix3 { TVector3 v_[3]; - + public: TMatrix3(); TMatrix3(const std::shared_ptr>& time_interval); diff --git a/include/fcl/math/motion/taylor_model/taylor_model.h b/include/fcl/math/motion/taylor_model/taylor_model.h index 8b5120318..e80561fd2 100644 --- a/include/fcl/math/motion/taylor_model/taylor_model.h +++ b/include/fcl/math/motion/taylor_model/taylor_model.h @@ -69,7 +69,7 @@ class TaylorModel public: void setTimeInterval(S l, S r); - + void setTimeInterval(const std::shared_ptr>& time_interval); const std::shared_ptr>& getTimeInterval() const; @@ -78,7 +78,7 @@ class TaylorModel S& coeff(std::size_t i); const Interval& remainder() const; Interval& remainder(); - + TaylorModel(); TaylorModel(const std::shared_ptr>& time_interval); TaylorModel(S coeff, const std::shared_ptr>& time_interval); diff --git a/include/fcl/math/motion/taylor_model/taylor_vector.h b/include/fcl/math/motion/taylor_model/taylor_vector.h index 72a18172b..884a3caab 100644 --- a/include/fcl/math/motion/taylor_model/taylor_vector.h +++ b/include/fcl/math/motion/taylor_model/taylor_vector.h @@ -50,13 +50,13 @@ class TVector3 TaylorModel i_[3]; public: - + TVector3(); TVector3(const std::shared_ptr>& time_interval); TVector3(TaylorModel v[3]); TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2); TVector3(const Vector3& v, const std::shared_ptr>& time_interval); - + TVector3 operator + (const TVector3& other) const; TVector3& operator += (const TVector3& other); diff --git a/include/fcl/math/sampler/sampler_se3_euler_ball.h b/include/fcl/math/sampler/sampler_se3_euler_ball.h index 610908a42..137d76606 100644 --- a/include/fcl/math/sampler/sampler_se3_euler_ball.h +++ b/include/fcl/math/sampler/sampler_se3_euler_ball.h @@ -53,7 +53,7 @@ class SamplerSE3Euler_ball : public SamplerBase SamplerSE3Euler_ball(S r_); void setBound(const S& r_); - + void getBound(S& r_) const; Vector6 sample() const; diff --git a/include/fcl/narrowphase/collision_request.h b/include/fcl/narrowphase/collision_request.h index fa85ac153..b1604f46c 100644 --- a/include/fcl/narrowphase/collision_request.h +++ b/include/fcl/narrowphase/collision_request.h @@ -81,7 +81,7 @@ struct CollisionRequest // single std::optional>. /// @brief If true, uses the provided initial guess for the GJK algorithm. bool enable_cached_gjk_guess; - + /// @brief The initial guess to use in the GJK algorithm. Vector3 cached_gjk_guess; diff --git a/include/fcl/narrowphase/collision_result.h b/include/fcl/narrowphase/collision_result.h index b7f36312c..8ce60cf9b 100644 --- a/include/fcl/narrowphase/collision_result.h +++ b/include/fcl/narrowphase/collision_result.h @@ -85,7 +85,7 @@ struct CollisionResult /// @brief get all the contacts void getContacts(std::vector>& contacts_); - /// @brief get all the cost sources + /// @brief get all the cost sources void getCostSources(std::vector>& cost_sources_); /// @brief clear the results obtained diff --git a/include/fcl/narrowphase/contact.h b/include/fcl/narrowphase/contact.h index ee3207916..686086c61 100644 --- a/include/fcl/narrowphase/contact.h +++ b/include/fcl/narrowphase/contact.h @@ -66,7 +66,7 @@ struct Contact /// if object 2 is octree, it is the query cell id (see /// OcTree::getNodeByQueryCellId) intptr_t b2; - + /// @brief contact normal, pointing from o1 to o2 Vector3 normal; @@ -76,7 +76,7 @@ struct Contact /// @brief penetration depth S penetration_depth; - + /// @brief invalid contact primitive information static const int NONE = -1; diff --git a/include/fcl/narrowphase/continuous_collision_request.h b/include/fcl/narrowphase/continuous_collision_request.h index f1a0f67ad..518b09598 100644 --- a/include/fcl/narrowphase/continuous_collision_request.h +++ b/include/fcl/narrowphase/continuous_collision_request.h @@ -65,13 +65,13 @@ struct ContinuousCollisionRequest /// @brief ccd solver type CCDSolverType ccd_solver_type; - + ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10, S toc_err_ = 0.0001, CCDMotionType ccd_motion_type_ = CCDM_TRANS, GJKSolverType gjk_solver_type_ = GST_LIBCCD, CCDSolverType ccd_solver_type_ = CCDC_NAIVE); - + }; using ContinuousCollisionRequestf = ContinuousCollisionRequest; diff --git a/include/fcl/narrowphase/continuous_collision_result.h b/include/fcl/narrowphase/continuous_collision_result.h index 473a856ec..8e5f8334a 100644 --- a/include/fcl/narrowphase/continuous_collision_result.h +++ b/include/fcl/narrowphase/continuous_collision_result.h @@ -49,14 +49,14 @@ struct ContinuousCollisionResult { /// @brief collision or not bool is_collide; - + /// @brief time of contact in [0, 1] S time_of_contact; Transform3 contact_tf1; Transform3 contact_tf2; - + ContinuousCollisionResult(); EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h index 8d81137f8..131dc7a7f 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h @@ -101,7 +101,7 @@ struct EPA public: enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed}; - + Status status; typename GJK::Simplex result; Vector3 normal; @@ -130,8 +130,8 @@ struct EPA Status evaluate(GJK& gjk, const Vector3& guess); - /// @brief the goal is to add a face connecting vertex w and face edge f[e] - bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); + /// @brief the goal is to add a face connecting vertex w and face edge f[e] + bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); }; using EPAf = EPA; diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h index 5fb481ffc..7949bba28 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h @@ -63,7 +63,7 @@ struct GJK { /// @brief simplex vertex SimplexV* c[4]; - /// @brief weight + /// @brief weight S p[4]; /// @brief size of simplex (number of vertices) size_t rank; @@ -79,7 +79,7 @@ struct GJK Simplex simplices[2]; GJK(unsigned int max_iterations_, S tolerance_); - + void initialize(); /// @brief GJK algorithm, given the initial value guess diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h index d3f967d07..6e073accf 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h @@ -63,14 +63,14 @@ struct MinkowskiDiff /// @brief rotation from shape0 to shape1 Matrix3 toshape1; - /// @brief transform from shape1 to shape0 + /// @brief transform from shape1 to shape0 Transform3 toshape0; MinkowskiDiff(); /// @brief support function for shape0 Vector3 support0(const Vector3& d) const; - + /// @brief support function for shape1 Vector3 support1(const Vector3& d) const; diff --git a/include/fcl/narrowphase/detail/failed_at_this_configuration.h b/include/fcl/narrowphase/detail/failed_at_this_configuration.h index bef56934a..1856fa2c3 100644 --- a/include/fcl/narrowphase/detail/failed_at_this_configuration.h +++ b/include/fcl/narrowphase/detail/failed_at_this_configuration.h @@ -87,7 +87,7 @@ FCL_API void ThrowFailedAtThisConfiguration( /** Helper class for propagating a low-level exception upwards but with configuration-specific details appended. The parameters - + @param s1 The first shape in the query. @param X_FS1 The pose of the first shape in frame F. @param s2 The second shape in the query. diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h index 9beef7625..ec58038b3 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h @@ -1,467 +1,467 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H -#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H - -#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API TriangleDistance; - -//============================================================================== -template -void TriangleDistance::segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, - Vector3& VEC, Vector3& X, Vector3& Y) -{ - Vector3 T; - S A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; - Vector3 TMP; - - T = Q - P; - A_dot_A = A.dot(A); - B_dot_B = B.dot(B); - A_dot_B = A.dot(B); - A_dot_T = A.dot(T); - B_dot_T = B.dot(T); - - // t parameterizes ray P,A - // u parameterizes ray Q,B - - S t, u; - - // compute t for the closest point on ray P,A to - // ray Q,B - - S denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; - - t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; - - // clamp result so t is on the segment P,A - - if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1; - - // find u for point on ray Q,B closest to point at t - - u = (t*A_dot_B - B_dot_T) / B_dot_B; - - // if u is on segment Q,B, t and u correspond to - // closest points, otherwise, clamp u, recompute and - // clamp t - - if((u <= 0) || std::isnan(u)) - { - Y = Q; - - t = A_dot_T / A_dot_A; - - if((t <= 0) || std::isnan(t)) - { - X = P; - VEC = Q - P; - } - else if(t >= 1) - { - X = P + A; - VEC = Q - X; - } - else - { - X = P + A * t; - TMP = T.cross(A); - VEC = A.cross(TMP); - } - } - else if (u >= 1) - { - Y = Q + B; - - t = (A_dot_B + A_dot_T) / A_dot_A; - - if((t <= 0) || std::isnan(t)) - { - X = P; - VEC = Y - P; - } - else if(t >= 1) - { - X = P + A; - VEC = Y - X; - } - else - { - X = P + A * t; - T = Y - P; - TMP = T.cross(A); - VEC= A.cross(TMP); - } - } - else - { - Y = Q + B * u; - - if((t <= 0) || std::isnan(t)) - { - X = P; - TMP = T.cross(B); - VEC = B.cross(TMP); - } - else if(t >= 1) - { - X = P + A; - T = Q - X; - TMP = T.cross(B); - VEC = B.cross(TMP); - } - else - { - X = P + A * t; - VEC = A.cross(B); - if(VEC.dot(T) < 0) - { - VEC = VEC * (-1); - } - } - } -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q) -{ - // Compute vectors along the 6 sides - - Vector3 Sv[3]; - Vector3 Tv[3]; - Vector3 VEC; - - Sv[0] = T1[1] - T1[0]; - Sv[1] = T1[2] - T1[1]; - Sv[2] = T1[0] - T1[2]; - - Tv[0] = T2[1] - T2[0]; - Tv[1] = T2[2] - T2[1]; - Tv[2] = T2[0] - T2[2]; - - // For each edge pair, the vector connecting the closest points - // of the edges defines a slab (parallel planes at head and tail - // enclose the slab). If we can show that the off-edge vertex of - // each triangle is outside of the slab, then the closest points - // of the edges are the closest points for the triangles. - // Even if these tests fail, it may be helpful to know the closest - // points found, and whether the triangles were shown disjoint - - Vector3 V; - Vector3 Z; - Vector3 minP = Vector3::Zero(); - Vector3 minQ = Vector3::Zero(); - S mindd; - int shown_disjoint = 0; - - mindd = (T1[0] - T2[0]).squaredNorm() + 1; // Set first minimum safely high - - for(int i = 0; i < 3; ++i) - { - for(int j = 0; j < 3; ++j) - { - // Find closest points on edges i & j, plus the - // vector (and distance squared) between these points - segPoints(T1[i], Sv[i], T2[j], Tv[j], VEC, P, Q); - - V = Q - P; - S dd = V.dot(V); - - // Verify this closest point pair only if the distance - // squared is less than the minimum found thus far. - - if(dd <= mindd) - { - minP = P; - minQ = Q; - mindd = dd; - - Z = T1[(i+2)%3] - P; - S a = Z.dot(VEC); - Z = T2[(j+2)%3] - Q; - S b = Z.dot(VEC); - - if((a <= 0) && (b >= 0)) return sqrt(dd); - - S p = V.dot(VEC); - - if(a < 0) a = 0; - if(b > 0) b = 0; - if((p - a + b) > 0) shown_disjoint = 1; - } - } - } - - // No edge pairs contained the closest points. - // either: - // 1. one of the closest points is a vertex, and the - // other point is interior to a face. - // 2. the triangles are overlapping. - // 3. an edge of one triangle is parallel to the other's face. If - // cases 1 and 2 are not true, then the closest points from the 9 - // edge pairs checks above can be taken as closest points for the - // triangles. - // 4. possibly, the triangles were degenerate. When the - // triangle points are nearly colinear or coincident, one - // of above tests might fail even though the edges tested - // contain the closest points. - - // First check for case 1 - - Vector3 Sn; - S Snl; - - Sn = Sv[0].cross(Sv[1]); // Compute normal to T1 triangle - Snl = Sn.dot(Sn); // Compute square of length of normal - - // If cross product is long enough, - - if(Snl > 1e-15) - { - // Get projection lengths of T2 points - - Vector3 Tp; - - V = T1[0] - T2[0]; - Tp[0] = V.dot(Sn); - - V = T1[0] - T2[1]; - Tp[1] = V.dot(Sn); - - V = T1[0] - T2[2]; - Tp[2] = V.dot(Sn); - - // If Sn is a separating direction, - // find point with smallest projection - - int point = -1; - if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) - { - if(Tp[0] < Tp[1]) point = 0; else point = 1; - if(Tp[2] < Tp[point]) point = 2; - } - else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) - { - if(Tp[0] > Tp[1]) point = 0; else point = 1; - if(Tp[2] > Tp[point]) point = 2; - } - - // If Sn is a separating direction, - - if(point >= 0) - { - shown_disjoint = 1; - - // Test whether the point found, when projected onto the - // other triangle, lies within the face. - - V = T2[point] - T1[0]; - Z = Sn.cross(Sv[0]); - if(V.dot(Z) > 0) - { - V = T2[point] - T1[1]; - Z = Sn.cross(Sv[1]); - if(V.dot(Z) > 0) - { - V = T2[point] - T1[2]; - Z = Sn.cross(Sv[2]); - if(V.dot(Z) > 0) - { - // T[point] passed the test - it's a closest point for - // the T2 triangle; the other point is on the face of T1 - P = T2[point] + Sn * (Tp[point] / Snl); - Q = T2[point]; - return (P - Q).norm(); - } - } - } - } - } - - Vector3 Tn; - S Tnl; - - Tn = Tv[0].cross(Tv[1]); - Tnl = Tn.dot(Tn); - - if(Tnl > 1e-15) - { - Vector3 Sp; - - V = T2[0] - T1[0]; - Sp[0] = V.dot(Tn); - - V = T2[0] - T1[1]; - Sp[1] = V.dot(Tn); - - V = T2[0] - T1[2]; - Sp[2] = V.dot(Tn); - - int point = -1; - if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) - { - if(Sp[0] < Sp[1]) point = 0; else point = 1; - if(Sp[2] < Sp[point]) point = 2; - } - else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) - { - if(Sp[0] > Sp[1]) point = 0; else point = 1; - if(Sp[2] > Sp[point]) point = 2; - } - - if(point >= 0) - { - shown_disjoint = 1; - - V = T1[point] - T2[0]; - Z = Tn.cross(Tv[0]); - if(V.dot(Z) > 0) - { - V = T1[point] - T2[1]; - Z = Tn.cross(Tv[1]); - if(V.dot(Z) > 0) - { - V = T1[point] - T2[2]; - Z = Tn.cross(Tv[2]); - if(V.dot(Z) > 0) - { - P = T1[point]; - Q = T1[point] + Tn * (Sp[point] / Tnl); - return (P - Q).norm(); - } - } - } - } - } - - // Case 1 can't be shown. - // If one of these tests showed the triangles disjoint, - // we assume case 3 or 4, otherwise we conclude case 2, - // that the triangles overlap. - - if(shown_disjoint) - { - P = minP; - Q = minQ; - return sqrt(mindd); - } - else return 0; -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - Vector3& P, Vector3& Q) -{ - Vector3 U[3]; - Vector3 T[3]; - U[0] = S1; U[1] = S2; U[2] = S3; - T[0] = T1; T[1] = T2; T[2] = T3; - - return triDistance(U, T, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q) -{ - Vector3 T_transformed[3]; - T_transformed[0] = R * T2[0] + Tl; - T_transformed[1] = R * T2[1] + Tl; - T_transformed[2] = R * T2[2] + Tl; - - return triDistance(T1, T_transformed, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Transform3& tf, - Vector3& P, Vector3& Q) -{ - Vector3 T_transformed[3]; - T_transformed[0] = tf * T2[0]; - T_transformed[1] = tf * T2[1]; - T_transformed[2] = tf * T2[2]; - - return triDistance(T1, T_transformed, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q) -{ - Vector3 T1_transformed = R * T1 + Tl; - Vector3 T2_transformed = R * T2 + Tl; - Vector3 T3_transformed = R * T3 + Tl; - return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - const Transform3& tf, - Vector3& P, Vector3& Q) -{ - Vector3 T1_transformed = tf * T1; - Vector3 T2_transformed = tf * T2; - Vector3 T3_transformed = tf * T3; - return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H +#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API TriangleDistance; + +//============================================================================== +template +void TriangleDistance::segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, + Vector3& VEC, Vector3& X, Vector3& Y) +{ + Vector3 T; + S A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; + Vector3 TMP; + + T = Q - P; + A_dot_A = A.dot(A); + B_dot_B = B.dot(B); + A_dot_B = A.dot(B); + A_dot_T = A.dot(T); + B_dot_T = B.dot(T); + + // t parameterizes ray P,A + // u parameterizes ray Q,B + + S t, u; + + // compute t for the closest point on ray P,A to + // ray Q,B + + S denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; + + t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; + + // clamp result so t is on the segment P,A + + if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1; + + // find u for point on ray Q,B closest to point at t + + u = (t*A_dot_B - B_dot_T) / B_dot_B; + + // if u is on segment Q,B, t and u correspond to + // closest points, otherwise, clamp u, recompute and + // clamp t + + if((u <= 0) || std::isnan(u)) + { + Y = Q; + + t = A_dot_T / A_dot_A; + + if((t <= 0) || std::isnan(t)) + { + X = P; + VEC = Q - P; + } + else if(t >= 1) + { + X = P + A; + VEC = Q - X; + } + else + { + X = P + A * t; + TMP = T.cross(A); + VEC = A.cross(TMP); + } + } + else if (u >= 1) + { + Y = Q + B; + + t = (A_dot_B + A_dot_T) / A_dot_A; + + if((t <= 0) || std::isnan(t)) + { + X = P; + VEC = Y - P; + } + else if(t >= 1) + { + X = P + A; + VEC = Y - X; + } + else + { + X = P + A * t; + T = Y - P; + TMP = T.cross(A); + VEC= A.cross(TMP); + } + } + else + { + Y = Q + B * u; + + if((t <= 0) || std::isnan(t)) + { + X = P; + TMP = T.cross(B); + VEC = B.cross(TMP); + } + else if(t >= 1) + { + X = P + A; + T = Q - X; + TMP = T.cross(B); + VEC = B.cross(TMP); + } + else + { + X = P + A * t; + VEC = A.cross(B); + if(VEC.dot(T) < 0) + { + VEC = VEC * (-1); + } + } + } +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q) +{ + // Compute vectors along the 6 sides + + Vector3 Sv[3]; + Vector3 Tv[3]; + Vector3 VEC; + + Sv[0] = T1[1] - T1[0]; + Sv[1] = T1[2] - T1[1]; + Sv[2] = T1[0] - T1[2]; + + Tv[0] = T2[1] - T2[0]; + Tv[1] = T2[2] - T2[1]; + Tv[2] = T2[0] - T2[2]; + + // For each edge pair, the vector connecting the closest points + // of the edges defines a slab (parallel planes at head and tail + // enclose the slab). If we can show that the off-edge vertex of + // each triangle is outside of the slab, then the closest points + // of the edges are the closest points for the triangles. + // Even if these tests fail, it may be helpful to know the closest + // points found, and whether the triangles were shown disjoint + + Vector3 V; + Vector3 Z; + Vector3 minP = Vector3::Zero(); + Vector3 minQ = Vector3::Zero(); + S mindd; + int shown_disjoint = 0; + + mindd = (T1[0] - T2[0]).squaredNorm() + 1; // Set first minimum safely high + + for(int i = 0; i < 3; ++i) + { + for(int j = 0; j < 3; ++j) + { + // Find closest points on edges i & j, plus the + // vector (and distance squared) between these points + segPoints(T1[i], Sv[i], T2[j], Tv[j], VEC, P, Q); + + V = Q - P; + S dd = V.dot(V); + + // Verify this closest point pair only if the distance + // squared is less than the minimum found thus far. + + if(dd <= mindd) + { + minP = P; + minQ = Q; + mindd = dd; + + Z = T1[(i+2)%3] - P; + S a = Z.dot(VEC); + Z = T2[(j+2)%3] - Q; + S b = Z.dot(VEC); + + if((a <= 0) && (b >= 0)) return sqrt(dd); + + S p = V.dot(VEC); + + if(a < 0) a = 0; + if(b > 0) b = 0; + if((p - a + b) > 0) shown_disjoint = 1; + } + } + } + + // No edge pairs contained the closest points. + // either: + // 1. one of the closest points is a vertex, and the + // other point is interior to a face. + // 2. the triangles are overlapping. + // 3. an edge of one triangle is parallel to the other's face. If + // cases 1 and 2 are not true, then the closest points from the 9 + // edge pairs checks above can be taken as closest points for the + // triangles. + // 4. possibly, the triangles were degenerate. When the + // triangle points are nearly colinear or coincident, one + // of above tests might fail even though the edges tested + // contain the closest points. + + // First check for case 1 + + Vector3 Sn; + S Snl; + + Sn = Sv[0].cross(Sv[1]); // Compute normal to T1 triangle + Snl = Sn.dot(Sn); // Compute square of length of normal + + // If cross product is long enough, + + if(Snl > 1e-15) + { + // Get projection lengths of T2 points + + Vector3 Tp; + + V = T1[0] - T2[0]; + Tp[0] = V.dot(Sn); + + V = T1[0] - T2[1]; + Tp[1] = V.dot(Sn); + + V = T1[0] - T2[2]; + Tp[2] = V.dot(Sn); + + // If Sn is a separating direction, + // find point with smallest projection + + int point = -1; + if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) + { + if(Tp[0] < Tp[1]) point = 0; else point = 1; + if(Tp[2] < Tp[point]) point = 2; + } + else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) + { + if(Tp[0] > Tp[1]) point = 0; else point = 1; + if(Tp[2] > Tp[point]) point = 2; + } + + // If Sn is a separating direction, + + if(point >= 0) + { + shown_disjoint = 1; + + // Test whether the point found, when projected onto the + // other triangle, lies within the face. + + V = T2[point] - T1[0]; + Z = Sn.cross(Sv[0]); + if(V.dot(Z) > 0) + { + V = T2[point] - T1[1]; + Z = Sn.cross(Sv[1]); + if(V.dot(Z) > 0) + { + V = T2[point] - T1[2]; + Z = Sn.cross(Sv[2]); + if(V.dot(Z) > 0) + { + // T[point] passed the test - it's a closest point for + // the T2 triangle; the other point is on the face of T1 + P = T2[point] + Sn * (Tp[point] / Snl); + Q = T2[point]; + return (P - Q).norm(); + } + } + } + } + } + + Vector3 Tn; + S Tnl; + + Tn = Tv[0].cross(Tv[1]); + Tnl = Tn.dot(Tn); + + if(Tnl > 1e-15) + { + Vector3 Sp; + + V = T2[0] - T1[0]; + Sp[0] = V.dot(Tn); + + V = T2[0] - T1[1]; + Sp[1] = V.dot(Tn); + + V = T2[0] - T1[2]; + Sp[2] = V.dot(Tn); + + int point = -1; + if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) + { + if(Sp[0] < Sp[1]) point = 0; else point = 1; + if(Sp[2] < Sp[point]) point = 2; + } + else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) + { + if(Sp[0] > Sp[1]) point = 0; else point = 1; + if(Sp[2] > Sp[point]) point = 2; + } + + if(point >= 0) + { + shown_disjoint = 1; + + V = T1[point] - T2[0]; + Z = Tn.cross(Tv[0]); + if(V.dot(Z) > 0) + { + V = T1[point] - T2[1]; + Z = Tn.cross(Tv[1]); + if(V.dot(Z) > 0) + { + V = T1[point] - T2[2]; + Z = Tn.cross(Tv[2]); + if(V.dot(Z) > 0) + { + P = T1[point]; + Q = T1[point] + Tn * (Sp[point] / Tnl); + return (P - Q).norm(); + } + } + } + } + } + + // Case 1 can't be shown. + // If one of these tests showed the triangles disjoint, + // we assume case 3 or 4, otherwise we conclude case 2, + // that the triangles overlap. + + if(shown_disjoint) + { + P = minP; + Q = minQ; + return sqrt(mindd); + } + else return 0; +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + Vector3& P, Vector3& Q) +{ + Vector3 U[3]; + Vector3 T[3]; + U[0] = S1; U[1] = S2; U[2] = S3; + T[0] = T1; T[1] = T2; T[2] = T3; + + return triDistance(U, T, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q) +{ + Vector3 T_transformed[3]; + T_transformed[0] = R * T2[0] + Tl; + T_transformed[1] = R * T2[1] + Tl; + T_transformed[2] = R * T2[2] + Tl; + + return triDistance(T1, T_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Transform3& tf, + Vector3& P, Vector3& Q) +{ + Vector3 T_transformed[3]; + T_transformed[0] = tf * T2[0]; + T_transformed[1] = tf * T2[1]; + T_transformed[2] = tf * T2[2]; + + return triDistance(T1, T_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q) +{ + Vector3 T1_transformed = R * T1 + Tl; + Vector3 T2_transformed = R * T2 + Tl; + Vector3 T3_transformed = R * T3 + Tl; + return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Transform3& tf, + Vector3& P, Vector3& Q) +{ + Vector3 T1_transformed = tf * T1; + Vector3 T2_transformed = tf * T2; + Vector3 T3_transformed = tf * T3; + return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h index b32408dc5..cc800f0f1 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h @@ -1,114 +1,114 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H -#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H - -#include "fcl/common/types.h" - -namespace fcl -{ - -namespace detail -{ - -/// @brief Triangle distance functions -template -class TriangleDistance -{ -public: - - /// @brief Returns closest points between an segment pair. - /// The first segment is P + t * A - /// The second segment is Q + t * B - /// X, Y are the closest points on the two segments - /// VEC is the vector between X and Y - static void segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, - Vector3& VEC, Vector3& X, Vector3& Y); - - /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them - /// T1 and T2 are two triangles - /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not - /// coincident points on the intersection of the triangles, as might be expected. - static S triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q); - - static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - Vector3& P, Vector3& Q); - - /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them - /// T1 and T2 are two triangles - /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not - /// coincident points on the intersection of the triangles, as might be expected. - /// The returned P and Q are both in the coordinate of the first triangle's coordinate - static S triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q); - - static S triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Transform3& tf, - Vector3& P, Vector3& Q); - - static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q); - - static S triDistance( - const Vector3& S1, - const Vector3& S2, - const Vector3& S3, - const Vector3& T1, - const Vector3& T2, - const Vector3& T3, - const Transform3& tf, - Vector3& P, - Vector3& Q); - -}; - -using TriangleDistancef = TriangleDistance; -using TriangleDistanced = TriangleDistance; - -} // namespace detail -} // namespace fcl - -#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H +#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H + +#include "fcl/common/types.h" + +namespace fcl +{ + +namespace detail +{ + +/// @brief Triangle distance functions +template +class TriangleDistance +{ +public: + + /// @brief Returns closest points between an segment pair. + /// The first segment is P + t * A + /// The second segment is Q + t * B + /// X, Y are the closest points on the two segments + /// VEC is the vector between X and Y + static void segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, + Vector3& VEC, Vector3& X, Vector3& Y); + + /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them + /// T1 and T2 are two triangles + /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not + /// coincident points on the intersection of the triangles, as might be expected. + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q); + + static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + Vector3& P, Vector3& Q); + + /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them + /// T1 and T2 are two triangles + /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not + /// coincident points on the intersection of the triangles, as might be expected. + /// The returned P and Q are both in the coordinate of the first triangle's coordinate + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q); + + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Transform3& tf, + Vector3& P, Vector3& Q); + + static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q); + + static S triDistance( + const Vector3& S1, + const Vector3& S2, + const Vector3& S3, + const Vector3& T1, + const Vector3& T2, + const Vector3& T3, + const Transform3& tf, + Vector3& P, + Vector3& Q); + +}; + +using TriangleDistancef = TriangleDistance; +using TriangleDistanced = TriangleDistance; + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" + +#endif diff --git a/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h index d1473867e..44cfe31f4 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h @@ -81,7 +81,7 @@ class BVHCollisionTraversalNode /// @brief BV culling test in one BVTT node bool BVTesting(int b1, int b2) const; - + /// @brief The first BVH model const BVHModel* model1; diff --git a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h index 959d532df..58ea12561 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h +++ b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h @@ -74,7 +74,7 @@ class CollisionTraversalNodeBase : public TraversalNodeBase /// @brief collision result kept during the traversal iteration CollisionResult* result; - /// @brief Whether stores statistics + /// @brief Whether stores statistics bool enable_statistics; }; diff --git a/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h b/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h index 7b7f02fda..027c791a9 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h @@ -1,1146 +1,1146 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H -#define FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H - -#include "fcl/narrowphase/detail/traversal/collision/intersect.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API Intersect; - -//============================================================================== -template -bool Intersect::isZero(S v) -{ - return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); -} - -//============================================================================== -/// @brief data: only used for EE, return the intersect point -template -bool Intersect::solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S& l, S& r, bool bVF, S coeffs[], Vector3* data) -{ - S v2[2]= {l*l,r*r}; - S v[2]= {l,r}; - S r_backup; - - unsigned char min3, min2, min1, max3, max2, max1; - - min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1; - min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1; - min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1; - - // bound the cubic - - S minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0]; - S major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0]; - - if(major<0) return false; - if(minor>0) return false; - - // starting here, the bounds have opposite values - S m = 0.5 * (r + l); - - // bound the derivative - S dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1]; - S dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1]; - - if((dminor > 0)||(dmajor < 0)) // we can use Newton - { - S m2 = m*m; - S fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0]; - S nl = m; - S nu = m; - if(fm>0) - { - nl-=(fm/dminor); - nu-=(fm/dmajor); - } - else - { - nu-=(fm/dminor); - nl-=(fm/dmajor); - } - - //intersect with [l,r] - - if(nl>r) return false; - if(nul) - { - if(nu -bool Intersect::insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p) -{ - Vector3 ab = b - a; - Vector3 ac = c - a; - Vector3 n = ab.cross(ac); - - Vector3 pa = a - p; - Vector3 pb = b - p; - Vector3 pc = c - p; - - if((pb.cross(pc)).dot(n) < -getEpsilon()) return false; - if((pc.cross(pa)).dot(n) < -getEpsilon()) return false; - if((pa.cross(pb)).dot(n) < -getEpsilon()) return false; - - return true; -} - -//============================================================================== -template -bool Intersect::insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p) -{ - return (p - a).dot(p - b) <= 0; -} - -//============================================================================== -/// @brief Calculate the line segment papb that is the shortest route between -/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where -/// pa = p1 + mua (p2 - p1) -/// pb = p3 + mub (p4 - p3) -/// Return FALSE if no solution exists. -template -bool Intersect::linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, - Vector3* pa, Vector3* pb, S* mua, S* mub) -{ - Vector3 p31 = p1 - p3; - Vector3 p34 = p4 - p3; - if(fabs(p34[0]) < getEpsilon() && fabs(p34[1]) < getEpsilon() && fabs(p34[2]) < getEpsilon()) - return false; - - Vector3 p12 = p2 - p1; - if(fabs(p12[0]) < getEpsilon() && fabs(p12[1]) < getEpsilon() && fabs(p12[2]) < getEpsilon()) - return false; - - S d3134 = p31.dot(p34); - S d3412 = p34.dot(p12); - S d3112 = p31.dot(p12); - S d3434 = p34.dot(p34); - S d1212 = p12.dot(p12); - - S denom = d1212 * d3434 - d3412 * d3412; - if(fabs(denom) < getEpsilon()) - return false; - S numer = d3134 * d3412 - d3112 * d3434; - - *mua = numer / denom; - if(*mua < 0 || *mua > 1) - return false; - - *mub = (d3134 + d3412 * (*mua)) / d3434; - if(*mub < 0 || *mub > 1) - return false; - - *pa = p1 + p12 * (*mua); - *pb = p3 + p34 * (*mub); - return true; -} - -//============================================================================== -template -bool Intersect::checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S t) -{ - return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t); -} - -//============================================================================== -template -bool Intersect::checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S t, Vector3* q_i) -{ - Vector3 a = a0 + va * t; - Vector3 b = b0 + vb * t; - Vector3 c = c0 + vc * t; - Vector3 d = d0 + vd * t; - Vector3 p1, p2; - S t_ab, t_cd; - if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd)) - { - if(q_i) *q_i = p1; - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - S t) -{ - return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t); -} - -//============================================================================== -template -bool Intersect::solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - bool bVF, - S* ret) -{ - S discriminant = b * b - 4 * a * c; - if(discriminant < 0) - return false; - - S sqrt_dis = sqrt(discriminant); - S r1 = (-b + sqrt_dis) / (2 * a); - bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false; - - S r2 = (-b - sqrt_dis) / (2 * a); - bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false; - - if(v1 && v2) - { - *ret = (r1 > r2) ? r2 : r1; - return true; - } - if(v1) - { - *ret = r1; - return true; - } - if(v2) - { - *ret = r2; - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp) -{ - if(isZero(a)) - { - S t = -c/b; - return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false; - } - - S discriminant = b*b-4*a*c; - if(discriminant < 0) - return false; - - S sqrt_dis = sqrt(discriminant); - - S r1 = (-b+sqrt_dis) / (2 * a); - bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false; - if(v1) return true; - - S r2 = (-b-sqrt_dis) / (2 * a); - bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false; - return v2; -} - -//============================================================================== -/// @brief Compute the cubic coefficients for VF case -/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. -template -void Intersect::computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S* a, S* b, S* c, S* d) -{ - Vector3 vavb = vb - va; - Vector3 vavc = vc - va; - Vector3 vavp = vp - va; - Vector3 a0b0 = b0 - a0; - Vector3 a0c0 = c0 - a0; - Vector3 a0p0 = p0 - a0; - - Vector3 vavb_cross_vavc = vavb.cross(vavc); - Vector3 vavb_cross_a0c0 = vavb.cross(a0c0); - Vector3 a0b0_cross_vavc = a0b0.cross(vavc); - Vector3 a0b0_cross_a0c0 = a0b0.cross(a0c0); - - *a = vavp.dot(vavb_cross_vavc); - *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); - *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); - *d = a0p0.dot(a0b0_cross_a0c0); -} - -//============================================================================== -template -void Intersect::computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S* a, S* b, S* c, S* d) -{ - Vector3 vavb = vb - va; - Vector3 vcvd = vd - vc; - Vector3 vavc = vc - va; - Vector3 c0d0 = d0 - c0; - Vector3 a0b0 = b0 - a0; - Vector3 a0c0 = c0 - a0; - Vector3 vavb_cross_vcvd = vavb.cross(vcvd); - Vector3 vavb_cross_c0d0 = vavb.cross(c0d0); - Vector3 a0b0_cross_vcvd = a0b0.cross(vcvd); - Vector3 a0b0_cross_c0d0 = a0b0.cross(c0d0); - - *a = vavc.dot(vavb_cross_vcvd); - *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); - *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); - *d = a0c0.dot(a0b0_cross_c0d0); -} - -//============================================================================== -template -void Intersect::computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - const Vector3& L, - S* a, S* b, S* c) -{ - Vector3 vbva = va - vb; - Vector3 vbvp = vp - vb; - Vector3 b0a0 = a0 - b0; - Vector3 b0p0 = p0 - b0; - - Vector3 L_cross_vbvp = L.cross(vbvp); - Vector3 L_cross_b0p0 = L.cross(b0p0); - - *a = L_cross_vbvp.dot(vbva); - *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); - *c = L_cross_b0p0.dot(b0a0); -} - -//============================================================================== -template -bool Intersect::intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - *collision_time = 2.0; - - Vector3 vp, va, vb, vc; - vp = p1 - p0; - va = a1 - a0; - vb = b1 - b0; - vc = c1 - c0; - - S a, b, c, d; - computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d); - - if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) - { - return false; - } - - - /// if(isZero(a)) - /// { - /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); - /// } - - S coeffs[4]; - coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; - - if(useNewton) - { - S l = 0; - S r = 1; - - if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs)) - { - *collision_time = 0.5 * (l + r); - } - } - else - { - S roots[3]; - int num = PolySolver::solveCubic(coeffs, roots); - for(int i = 0; i < num; ++i) - { - S r = roots[i]; - if(r < 0 || r > 1) continue; - if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r)) - { - *collision_time = r; - break; - } - } - } - - if(*collision_time > 1) - { - return false; - } - - *p_i = vp * (*collision_time) + p0; - return true; -} - -//============================================================================== -template -bool Intersect::intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - *collision_time = 2.0; - - Vector3 va, vb, vc, vd; - va = a1 - a0; - vb = b1 - b0; - vc = c1 - c0; - vd = d1 - d0; - - S a, b, c, d; - computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d); - - if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) - { - return false; - } - - /// if(isZero(a)) - /// { - /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); - /// } - - - S coeffs[4]; - coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; - - if(useNewton) - { - S l = 0; - S r = 1; - - if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i)) - { - *collision_time = (l + r) * 0.5; - } - } - else - { - S roots[3]; - int num = PolySolver::solveCubic(coeffs, roots); - for(int i = 0; i < num; ++i) - { - S r = roots[i]; - if(r < 0 || r > 1) continue; - - if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i)) - { - *collision_time = r; - break; - } - } - } - - if(*collision_time > 1) - { - return false; - } - - return true; -} - -//============================================================================== -template -bool Intersect::intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& p1, - const Vector3& L) -{ - Vector3 va, vb, vp; - va = a1 - a0; - vb = b1 - b0; - vp = p1 - p0; - - S a, b, c; - computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c); - - if(isZero(a) && isZero(b) && isZero(c)) - return true; - - return solveSquare(a, b, c, a0, b0, p0, va, vb, vp); - -} - -//============================================================================== -/// @brief Prefilter for intersection, works for both VF and EE -template -bool Intersect::intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1) -{ - Vector3 n0 = (b0 - a0).cross(c0 - a0); - Vector3 n1 = (b1 - a1).cross(c1 - a1); - Vector3 a0a1 = a1 - a0; - Vector3 b0b1 = b1 - b0; - Vector3 c0c1 = c1 - c0; - Vector3 delta = (b0b1 - a0a1).cross(c0c1 - a0a1); - Vector3 nx = (n0 + n1 - delta) * 0.5; - - Vector3 a0d0 = d0 - a0; - Vector3 a1d1 = d1 - a1; - - S A = n0.dot(a0d0); - S B = n1.dot(a1d1); - S C = nx.dot(a0d0); - S D = nx.dot(a1d1); - S E = n1.dot(a0d0); - S F = n0.dot(a1d1); - - if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0) - return false; - if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0) - return false; - - return true; -} - -//============================================================================== -template -bool Intersect::intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1)) - { - return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton); - } - else - return false; -} - -//============================================================================== -template -bool Intersect::intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1)) - { - return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton); - } - else - return false; -} - -//============================================================================== -template -bool Intersect::intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Matrix3& R, - const Vector3& T, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 Q1_ = R * Q1 + T; - Vector3 Q2_ = R * Q2 + T; - Vector3 Q3_ = R * Q3 + T; - - return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); -} - -//============================================================================== -template -bool Intersect::intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Transform3& tf, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 Q1_ = tf * Q1; - Vector3 Q2_ = tf * Q2; - Vector3 Q3_ = tf * Q3; - - return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); -} - -//============================================================================== -template -bool Intersect::intersect_Triangle_ODE_style( - const Vector3& P1, const Vector3& P2, const Vector3& P3, - const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 n1; - S t1; - bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1); - if(!b1) return false; - - Vector3 n2; - S t2; - bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); - if(!b2) return false; - - if(sameSideOfPlane(P1, P2, P3, n2, t2)) - return false; - - if(sameSideOfPlane(Q1, Q2, Q3, n1, t1)) - return false; - - Vector3 clipped_points1[getMaxTriangleClips()]; - unsigned int num_clipped_points1 = 0; - Vector3 clipped_points2[getMaxTriangleClips()]; - unsigned int num_clipped_points2 = 0; - - Vector3 deepest_points1[getMaxTriangleClips()]; - unsigned int num_deepest_points1 = 0; - Vector3 deepest_points2[getMaxTriangleClips()]; - unsigned int num_deepest_points2 = 0; - S penetration_depth1 = -1, penetration_depth2 = -1; - - clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2); - - if(num_clipped_points2 == 0) - return false; - - computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); - if(num_deepest_points2 == 0) - return false; - - clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1); - if(num_clipped_points1 == 0) - return false; - - computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); - if(num_deepest_points1 == 0) - return false; - - - /// Return contact information - if(contact_points && num_contact_points && penetration_depth && normal) - { - if(penetration_depth1 > penetration_depth2) - { - *num_contact_points = num_deepest_points2; - for(unsigned int i = 0; i < num_deepest_points2; ++i) - { - contact_points[i] = deepest_points2[i]; - } - - *normal = n1; - *penetration_depth = penetration_depth2; - } - else - { - *num_contact_points = num_deepest_points1; - for(unsigned int i = 0; i < num_deepest_points1; ++i) - { - contact_points[i] = deepest_points1[i]; - } - - *normal = -n2; - *penetration_depth = penetration_depth1; - } - } - - return true; -} - -//============================================================================== -template -bool Intersect::intersect_Triangle( - const Vector3& P1, const Vector3& P2, const Vector3& P3, - const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 p1 = P1 - P1; - Vector3 p2 = P2 - P1; - Vector3 p3 = P3 - P1; - Vector3 q1 = Q1 - P1; - Vector3 q2 = Q2 - P1; - Vector3 q3 = Q3 - P1; - - Vector3 e1 = p2 - p1; - Vector3 e2 = p3 - p2; - Vector3 n1 = e1.cross(e2); - if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 f1 = q2 - q1; - Vector3 f2 = q3 - q2; - Vector3 m1 = f1.cross(f2); - if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef11 = e1.cross(f1); - if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef12 = e1.cross(f2); - if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 f3 = q1 - q3; - Vector3 ef13 = e1.cross(f3); - if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef21 = e2.cross(f1); - if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef22 = e2.cross(f2); - if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef23 = e2.cross(f3); - if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 e3 = p1 - p3; - Vector3 ef31 = e3.cross(f1); - if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef32 = e3.cross(f2); - if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef33 = e3.cross(f3); - if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 g1 = e1.cross(n1); - if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 g2 = e2.cross(n1); - if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 g3 = e3.cross(n1); - if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 h1 = f1.cross(m1); - if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 h2 = f2.cross(m1); - if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 h3 = f3.cross(m1); - if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false; - - if(contact_points && num_contact_points && penetration_depth && normal) - { - Vector3 n1, n2; - S t1, t2; - buildTrianglePlane(P1, P2, P3, &n1, &t1); - buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); - - Vector3 deepest_points1[3]; - unsigned int num_deepest_points1 = 0; - Vector3 deepest_points2[3]; - unsigned int num_deepest_points2 = 0; - S penetration_depth1, penetration_depth2; - - Vector3 P[3] = {P1, P2, P3}; - Vector3 Q[3] = {Q1, Q2, Q3}; - - computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); - computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); - - - if(penetration_depth1 > penetration_depth2) - { - *num_contact_points = std::min(num_deepest_points2, (unsigned int)2); - for(unsigned int i = 0; i < *num_contact_points; ++i) - { - contact_points[i] = deepest_points2[i]; - } - - *normal = n1; - *penetration_depth = penetration_depth2; - } - else - { - *num_contact_points = std::min(num_deepest_points1, (unsigned int)2); - for(unsigned int i = 0; i < *num_contact_points; ++i) - { - contact_points[i] = deepest_points1[i]; - } - - *normal = -n2; - *penetration_depth = penetration_depth1; - } - } - - return true; -} - -//============================================================================== -template -void Intersect::computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points) -{ - *num_deepest_points = 0; - S max_depth = -std::numeric_limits::max(); - unsigned int num_deepest_points_ = 0; - unsigned int num_neg = 0; - unsigned int num_pos = 0; - unsigned int num_zero = 0; - - for(unsigned int i = 0; i < num_clipped_points; ++i) - { - S dist = -distanceToPlane(n, t, clipped_points[i]); - if(dist > getEpsilon()) num_pos++; - else if(dist < -getEpsilon()) num_neg++; - else num_zero++; - if(dist > max_depth) - { - max_depth = dist; - num_deepest_points_ = 1; - deepest_points[num_deepest_points_ - 1] = clipped_points[i]; - } - else if(dist + 1e-6 >= max_depth) - { - num_deepest_points_++; - deepest_points[num_deepest_points_ - 1] = clipped_points[i]; - } - } - - if(max_depth < -getEpsilon()) - num_deepest_points_ = 0; - - if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0))) - num_deepest_points_ = 0; - - *penetration_depth = max_depth; - *num_deepest_points = num_deepest_points_; -} - -//============================================================================== -template -void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, - const Vector3& t1, const Vector3& t2, const Vector3& t3, - const Vector3& tn, S to, - Vector3 clipped_points[], unsigned int* num_clipped_points, - bool clip_triangle) -{ - *num_clipped_points = 0; - Vector3 temp_clip[getMaxTriangleClips()]; - Vector3 temp_clip2[getMaxTriangleClips()]; - unsigned int num_temp_clip = 0; - unsigned int num_temp_clip2 = 0; - Vector3 v[3] = {v1, v2, v3}; - - Vector3 plane_n; - S plane_dist; - - if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist)) - { - clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip); - if(num_temp_clip > 0) - { - if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist)) - { - clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2); - if(num_temp_clip2 > 0) - { - if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist)) - { - if(clip_triangle) - { - num_temp_clip = 0; - clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip); - if(num_temp_clip > 0) - { - clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points); - } - } - else - { - clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points); - } - } - } - } - } - } -} - -//============================================================================== -template -void Intersect::clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points) -{ - *num_clipped_points = 0; - - unsigned int num_clipped_points_ = 0; - unsigned int vi; - unsigned int prev_classify = 2; - unsigned int classify; - for(unsigned int i = 0; i <= num_polygon_points; ++i) - { - vi = (i % num_polygon_points); - S d = distanceToPlane(n, t, polygon_points[i]); - classify = ((d > getEpsilon()) ? 1 : 0); - if(classify == 0) - { - if(prev_classify == 1) - { - if(num_clipped_points_ < getMaxTriangleClips()) - { - Vector3 tmp; - clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); - if(num_clipped_points_ > 0) - { - if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - else - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - } - - if(num_clipped_points_ < getMaxTriangleClips() && i < num_polygon_points) - { - clipped_points[num_clipped_points_] = polygon_points[vi]; - num_clipped_points_++; - } - } - else - { - if(prev_classify == 0) - { - if(num_clipped_points_ < getMaxTriangleClips()) - { - Vector3 tmp; - clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); - if(num_clipped_points_ > 0) - { - if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - else - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - } - } - - prev_classify = classify; - } - - if(num_clipped_points_ > 2) - { - if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < getEpsilon()) - { - num_clipped_points_--; - } - } - - *num_clipped_points = num_clipped_points_; -} - -//============================================================================== -template -void Intersect::clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point) -{ - S dist1 = distanceToPlane(n, t, v1); - Vector3 tmp = v2 - v1; - S dist2 = tmp.dot(n); - *clipped_point = tmp * (-dist1 / dist2) + v1; -} - -//============================================================================== -template -S Intersect::distanceToPlane(const Vector3& n, S t, const Vector3& v) -{ - return n.dot(v) - t; -} - -//============================================================================== -template -bool Intersect::buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t) -{ - Vector3 n_ = (v2 - v1).cross(v3 - v1); - bool can_normalize = false; - normalize(n_, &can_normalize); - if(can_normalize) - { - *n = n_; - *t = n_.dot(v1); - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t) -{ - Vector3 n_ = (v2 - v1).cross(tn); - bool can_normalize = false; - normalize(n_, &can_normalize); - if(can_normalize) - { - *n = n_; - *t = n_.dot(v1); - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t) -{ - S dist1 = distanceToPlane(n, t, v1); - S dist2 = dist1 * distanceToPlane(n, t, v2); - S dist3 = dist1 * distanceToPlane(n, t, v3); - if((dist2 > 0) && (dist3 > 0)) - return true; - return false; -} - -//============================================================================== -template -int Intersect::project6(const Vector3& ax, - const Vector3& p1, const Vector3& p2, const Vector3& p3, - const Vector3& q1, const Vector3& q2, const Vector3& q3) -{ - S P1 = ax.dot(p1); - S P2 = ax.dot(p2); - S P3 = ax.dot(p3); - S Q1 = ax.dot(q1); - S Q2 = ax.dot(q2); - S Q3 = ax.dot(q3); - - S mn1 = std::min(P1, std::min(P2, P3)); - S mx2 = std::max(Q1, std::max(Q2, Q3)); - if(mn1 > mx2) return 0; - - S mx1 = std::max(P1, std::max(P2, P3)); - S mn2 = std::min(Q1, std::min(Q2, Q3)); - - if(mn2 > mx1) return 0; - return 1; -} - -//============================================================================== -template -S Intersect::gaussianCDF(S x) -{ - return 0.5 * std::erfc(-x / sqrt(2.0)); -} - -//============================================================================== -template -constexpr S Intersect::getEpsilon() -{ - return 1e-5; -} - -//============================================================================== -template -constexpr S Intersect::getNearZeroThreshold() -{ - return 1e-7; -} - -//============================================================================== -template -constexpr S Intersect::getCcdResolution() -{ - return 1e-7; -} - -//============================================================================== -template -constexpr unsigned int Intersect::getMaxTriangleClips() -{ - return 8; -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H +#define FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H + +#include "fcl/narrowphase/detail/traversal/collision/intersect.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API Intersect; + +//============================================================================== +template +bool Intersect::isZero(S v) +{ + return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); +} + +//============================================================================== +/// @brief data: only used for EE, return the intersect point +template +bool Intersect::solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S& l, S& r, bool bVF, S coeffs[], Vector3* data) +{ + S v2[2]= {l*l,r*r}; + S v[2]= {l,r}; + S r_backup; + + unsigned char min3, min2, min1, max3, max2, max1; + + min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1; + min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1; + min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1; + + // bound the cubic + + S minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0]; + S major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0]; + + if(major<0) return false; + if(minor>0) return false; + + // starting here, the bounds have opposite values + S m = 0.5 * (r + l); + + // bound the derivative + S dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1]; + S dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1]; + + if((dminor > 0)||(dmajor < 0)) // we can use Newton + { + S m2 = m*m; + S fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0]; + S nl = m; + S nu = m; + if(fm>0) + { + nl-=(fm/dminor); + nu-=(fm/dmajor); + } + else + { + nu-=(fm/dminor); + nl-=(fm/dmajor); + } + + //intersect with [l,r] + + if(nl>r) return false; + if(nul) + { + if(nu +bool Intersect::insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p) +{ + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 n = ab.cross(ac); + + Vector3 pa = a - p; + Vector3 pb = b - p; + Vector3 pc = c - p; + + if((pb.cross(pc)).dot(n) < -getEpsilon()) return false; + if((pc.cross(pa)).dot(n) < -getEpsilon()) return false; + if((pa.cross(pb)).dot(n) < -getEpsilon()) return false; + + return true; +} + +//============================================================================== +template +bool Intersect::insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p) +{ + return (p - a).dot(p - b) <= 0; +} + +//============================================================================== +/// @brief Calculate the line segment papb that is the shortest route between +/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where +/// pa = p1 + mua (p2 - p1) +/// pb = p3 + mub (p4 - p3) +/// Return FALSE if no solution exists. +template +bool Intersect::linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, + Vector3* pa, Vector3* pb, S* mua, S* mub) +{ + Vector3 p31 = p1 - p3; + Vector3 p34 = p4 - p3; + if(fabs(p34[0]) < getEpsilon() && fabs(p34[1]) < getEpsilon() && fabs(p34[2]) < getEpsilon()) + return false; + + Vector3 p12 = p2 - p1; + if(fabs(p12[0]) < getEpsilon() && fabs(p12[1]) < getEpsilon() && fabs(p12[2]) < getEpsilon()) + return false; + + S d3134 = p31.dot(p34); + S d3412 = p34.dot(p12); + S d3112 = p31.dot(p12); + S d3434 = p34.dot(p34); + S d1212 = p12.dot(p12); + + S denom = d1212 * d3434 - d3412 * d3412; + if(fabs(denom) < getEpsilon()) + return false; + S numer = d3134 * d3412 - d3112 * d3434; + + *mua = numer / denom; + if(*mua < 0 || *mua > 1) + return false; + + *mub = (d3134 + d3412 * (*mua)) / d3434; + if(*mub < 0 || *mub > 1) + return false; + + *pa = p1 + p12 * (*mua); + *pb = p3 + p34 * (*mub); + return true; +} + +//============================================================================== +template +bool Intersect::checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S t) +{ + return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t); +} + +//============================================================================== +template +bool Intersect::checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S t, Vector3* q_i) +{ + Vector3 a = a0 + va * t; + Vector3 b = b0 + vb * t; + Vector3 c = c0 + vc * t; + Vector3 d = d0 + vd * t; + Vector3 p1, p2; + S t_ab, t_cd; + if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd)) + { + if(q_i) *q_i = p1; + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + S t) +{ + return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t); +} + +//============================================================================== +template +bool Intersect::solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + bool bVF, + S* ret) +{ + S discriminant = b * b - 4 * a * c; + if(discriminant < 0) + return false; + + S sqrt_dis = sqrt(discriminant); + S r1 = (-b + sqrt_dis) / (2 * a); + bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false; + + S r2 = (-b - sqrt_dis) / (2 * a); + bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false; + + if(v1 && v2) + { + *ret = (r1 > r2) ? r2 : r1; + return true; + } + if(v1) + { + *ret = r1; + return true; + } + if(v2) + { + *ret = r2; + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp) +{ + if(isZero(a)) + { + S t = -c/b; + return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false; + } + + S discriminant = b*b-4*a*c; + if(discriminant < 0) + return false; + + S sqrt_dis = sqrt(discriminant); + + S r1 = (-b+sqrt_dis) / (2 * a); + bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false; + if(v1) return true; + + S r2 = (-b-sqrt_dis) / (2 * a); + bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false; + return v2; +} + +//============================================================================== +/// @brief Compute the cubic coefficients for VF case +/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. +template +void Intersect::computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S* a, S* b, S* c, S* d) +{ + Vector3 vavb = vb - va; + Vector3 vavc = vc - va; + Vector3 vavp = vp - va; + Vector3 a0b0 = b0 - a0; + Vector3 a0c0 = c0 - a0; + Vector3 a0p0 = p0 - a0; + + Vector3 vavb_cross_vavc = vavb.cross(vavc); + Vector3 vavb_cross_a0c0 = vavb.cross(a0c0); + Vector3 a0b0_cross_vavc = a0b0.cross(vavc); + Vector3 a0b0_cross_a0c0 = a0b0.cross(a0c0); + + *a = vavp.dot(vavb_cross_vavc); + *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); + *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); + *d = a0p0.dot(a0b0_cross_a0c0); +} + +//============================================================================== +template +void Intersect::computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S* a, S* b, S* c, S* d) +{ + Vector3 vavb = vb - va; + Vector3 vcvd = vd - vc; + Vector3 vavc = vc - va; + Vector3 c0d0 = d0 - c0; + Vector3 a0b0 = b0 - a0; + Vector3 a0c0 = c0 - a0; + Vector3 vavb_cross_vcvd = vavb.cross(vcvd); + Vector3 vavb_cross_c0d0 = vavb.cross(c0d0); + Vector3 a0b0_cross_vcvd = a0b0.cross(vcvd); + Vector3 a0b0_cross_c0d0 = a0b0.cross(c0d0); + + *a = vavc.dot(vavb_cross_vcvd); + *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); + *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); + *d = a0c0.dot(a0b0_cross_c0d0); +} + +//============================================================================== +template +void Intersect::computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + const Vector3& L, + S* a, S* b, S* c) +{ + Vector3 vbva = va - vb; + Vector3 vbvp = vp - vb; + Vector3 b0a0 = a0 - b0; + Vector3 b0p0 = p0 - b0; + + Vector3 L_cross_vbvp = L.cross(vbvp); + Vector3 L_cross_b0p0 = L.cross(b0p0); + + *a = L_cross_vbvp.dot(vbva); + *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); + *c = L_cross_b0p0.dot(b0a0); +} + +//============================================================================== +template +bool Intersect::intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + *collision_time = 2.0; + + Vector3 vp, va, vb, vc; + vp = p1 - p0; + va = a1 - a0; + vb = b1 - b0; + vc = c1 - c0; + + S a, b, c, d; + computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d); + + if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) + { + return false; + } + + + /// if(isZero(a)) + /// { + /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); + /// } + + S coeffs[4]; + coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; + + if(useNewton) + { + S l = 0; + S r = 1; + + if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs)) + { + *collision_time = 0.5 * (l + r); + } + } + else + { + S roots[3]; + int num = PolySolver::solveCubic(coeffs, roots); + for(int i = 0; i < num; ++i) + { + S r = roots[i]; + if(r < 0 || r > 1) continue; + if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r)) + { + *collision_time = r; + break; + } + } + } + + if(*collision_time > 1) + { + return false; + } + + *p_i = vp * (*collision_time) + p0; + return true; +} + +//============================================================================== +template +bool Intersect::intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + *collision_time = 2.0; + + Vector3 va, vb, vc, vd; + va = a1 - a0; + vb = b1 - b0; + vc = c1 - c0; + vd = d1 - d0; + + S a, b, c, d; + computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d); + + if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) + { + return false; + } + + /// if(isZero(a)) + /// { + /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); + /// } + + + S coeffs[4]; + coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; + + if(useNewton) + { + S l = 0; + S r = 1; + + if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i)) + { + *collision_time = (l + r) * 0.5; + } + } + else + { + S roots[3]; + int num = PolySolver::solveCubic(coeffs, roots); + for(int i = 0; i < num; ++i) + { + S r = roots[i]; + if(r < 0 || r > 1) continue; + + if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i)) + { + *collision_time = r; + break; + } + } + } + + if(*collision_time > 1) + { + return false; + } + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& p1, + const Vector3& L) +{ + Vector3 va, vb, vp; + va = a1 - a0; + vb = b1 - b0; + vp = p1 - p0; + + S a, b, c; + computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c); + + if(isZero(a) && isZero(b) && isZero(c)) + return true; + + return solveSquare(a, b, c, a0, b0, p0, va, vb, vp); + +} + +//============================================================================== +/// @brief Prefilter for intersection, works for both VF and EE +template +bool Intersect::intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1) +{ + Vector3 n0 = (b0 - a0).cross(c0 - a0); + Vector3 n1 = (b1 - a1).cross(c1 - a1); + Vector3 a0a1 = a1 - a0; + Vector3 b0b1 = b1 - b0; + Vector3 c0c1 = c1 - c0; + Vector3 delta = (b0b1 - a0a1).cross(c0c1 - a0a1); + Vector3 nx = (n0 + n1 - delta) * 0.5; + + Vector3 a0d0 = d0 - a0; + Vector3 a1d1 = d1 - a1; + + S A = n0.dot(a0d0); + S B = n1.dot(a1d1); + S C = nx.dot(a0d0); + S D = nx.dot(a1d1); + S E = n1.dot(a0d0); + S F = n0.dot(a1d1); + + if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0) + return false; + if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0) + return false; + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1)) + { + return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton); + } + else + return false; +} + +//============================================================================== +template +bool Intersect::intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1)) + { + return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton); + } + else + return false; +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Matrix3& R, + const Vector3& T, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 Q1_ = R * Q1 + T; + Vector3 Q2_ = R * Q2 + T; + Vector3 Q3_ = R * Q3 + T; + + return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Transform3& tf, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 Q1_ = tf * Q1; + Vector3 Q2_ = tf * Q2; + Vector3 Q3_ = tf * Q3; + + return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); +} + +//============================================================================== +template +bool Intersect::intersect_Triangle_ODE_style( + const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 n1; + S t1; + bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1); + if(!b1) return false; + + Vector3 n2; + S t2; + bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); + if(!b2) return false; + + if(sameSideOfPlane(P1, P2, P3, n2, t2)) + return false; + + if(sameSideOfPlane(Q1, Q2, Q3, n1, t1)) + return false; + + Vector3 clipped_points1[getMaxTriangleClips()]; + unsigned int num_clipped_points1 = 0; + Vector3 clipped_points2[getMaxTriangleClips()]; + unsigned int num_clipped_points2 = 0; + + Vector3 deepest_points1[getMaxTriangleClips()]; + unsigned int num_deepest_points1 = 0; + Vector3 deepest_points2[getMaxTriangleClips()]; + unsigned int num_deepest_points2 = 0; + S penetration_depth1 = -1, penetration_depth2 = -1; + + clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2); + + if(num_clipped_points2 == 0) + return false; + + computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); + if(num_deepest_points2 == 0) + return false; + + clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1); + if(num_clipped_points1 == 0) + return false; + + computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); + if(num_deepest_points1 == 0) + return false; + + + /// Return contact information + if(contact_points && num_contact_points && penetration_depth && normal) + { + if(penetration_depth1 > penetration_depth2) + { + *num_contact_points = num_deepest_points2; + for(unsigned int i = 0; i < num_deepest_points2; ++i) + { + contact_points[i] = deepest_points2[i]; + } + + *normal = n1; + *penetration_depth = penetration_depth2; + } + else + { + *num_contact_points = num_deepest_points1; + for(unsigned int i = 0; i < num_deepest_points1; ++i) + { + contact_points[i] = deepest_points1[i]; + } + + *normal = -n2; + *penetration_depth = penetration_depth1; + } + } + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 p1 = P1 - P1; + Vector3 p2 = P2 - P1; + Vector3 p3 = P3 - P1; + Vector3 q1 = Q1 - P1; + Vector3 q2 = Q2 - P1; + Vector3 q3 = Q3 - P1; + + Vector3 e1 = p2 - p1; + Vector3 e2 = p3 - p2; + Vector3 n1 = e1.cross(e2); + if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 f1 = q2 - q1; + Vector3 f2 = q3 - q2; + Vector3 m1 = f1.cross(f2); + if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef11 = e1.cross(f1); + if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef12 = e1.cross(f2); + if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 f3 = q1 - q3; + Vector3 ef13 = e1.cross(f3); + if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef21 = e2.cross(f1); + if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef22 = e2.cross(f2); + if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef23 = e2.cross(f3); + if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 e3 = p1 - p3; + Vector3 ef31 = e3.cross(f1); + if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef32 = e3.cross(f2); + if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef33 = e3.cross(f3); + if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g1 = e1.cross(n1); + if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g2 = e2.cross(n1); + if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g3 = e3.cross(n1); + if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h1 = f1.cross(m1); + if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h2 = f2.cross(m1); + if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h3 = f3.cross(m1); + if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false; + + if(contact_points && num_contact_points && penetration_depth && normal) + { + Vector3 n1, n2; + S t1, t2; + buildTrianglePlane(P1, P2, P3, &n1, &t1); + buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); + + Vector3 deepest_points1[3]; + unsigned int num_deepest_points1 = 0; + Vector3 deepest_points2[3]; + unsigned int num_deepest_points2 = 0; + S penetration_depth1, penetration_depth2; + + Vector3 P[3] = {P1, P2, P3}; + Vector3 Q[3] = {Q1, Q2, Q3}; + + computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); + computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); + + + if(penetration_depth1 > penetration_depth2) + { + *num_contact_points = std::min(num_deepest_points2, (unsigned int)2); + for(unsigned int i = 0; i < *num_contact_points; ++i) + { + contact_points[i] = deepest_points2[i]; + } + + *normal = n1; + *penetration_depth = penetration_depth2; + } + else + { + *num_contact_points = std::min(num_deepest_points1, (unsigned int)2); + for(unsigned int i = 0; i < *num_contact_points; ++i) + { + contact_points[i] = deepest_points1[i]; + } + + *normal = -n2; + *penetration_depth = penetration_depth1; + } + } + + return true; +} + +//============================================================================== +template +void Intersect::computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points) +{ + *num_deepest_points = 0; + S max_depth = -std::numeric_limits::max(); + unsigned int num_deepest_points_ = 0; + unsigned int num_neg = 0; + unsigned int num_pos = 0; + unsigned int num_zero = 0; + + for(unsigned int i = 0; i < num_clipped_points; ++i) + { + S dist = -distanceToPlane(n, t, clipped_points[i]); + if(dist > getEpsilon()) num_pos++; + else if(dist < -getEpsilon()) num_neg++; + else num_zero++; + if(dist > max_depth) + { + max_depth = dist; + num_deepest_points_ = 1; + deepest_points[num_deepest_points_ - 1] = clipped_points[i]; + } + else if(dist + 1e-6 >= max_depth) + { + num_deepest_points_++; + deepest_points[num_deepest_points_ - 1] = clipped_points[i]; + } + } + + if(max_depth < -getEpsilon()) + num_deepest_points_ = 0; + + if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0))) + num_deepest_points_ = 0; + + *penetration_depth = max_depth; + *num_deepest_points = num_deepest_points_; +} + +//============================================================================== +template +void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, + const Vector3& t1, const Vector3& t2, const Vector3& t3, + const Vector3& tn, S to, + Vector3 clipped_points[], unsigned int* num_clipped_points, + bool clip_triangle) +{ + *num_clipped_points = 0; + Vector3 temp_clip[getMaxTriangleClips()]; + Vector3 temp_clip2[getMaxTriangleClips()]; + unsigned int num_temp_clip = 0; + unsigned int num_temp_clip2 = 0; + Vector3 v[3] = {v1, v2, v3}; + + Vector3 plane_n; + S plane_dist; + + if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist)) + { + clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip); + if(num_temp_clip > 0) + { + if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist)) + { + clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2); + if(num_temp_clip2 > 0) + { + if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist)) + { + if(clip_triangle) + { + num_temp_clip = 0; + clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip); + if(num_temp_clip > 0) + { + clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points); + } + } + else + { + clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points); + } + } + } + } + } + } +} + +//============================================================================== +template +void Intersect::clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points) +{ + *num_clipped_points = 0; + + unsigned int num_clipped_points_ = 0; + unsigned int vi; + unsigned int prev_classify = 2; + unsigned int classify; + for(unsigned int i = 0; i <= num_polygon_points; ++i) + { + vi = (i % num_polygon_points); + S d = distanceToPlane(n, t, polygon_points[i]); + classify = ((d > getEpsilon()) ? 1 : 0); + if(classify == 0) + { + if(prev_classify == 1) + { + if(num_clipped_points_ < getMaxTriangleClips()) + { + Vector3 tmp; + clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); + if(num_clipped_points_ > 0) + { + if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + else + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + } + + if(num_clipped_points_ < getMaxTriangleClips() && i < num_polygon_points) + { + clipped_points[num_clipped_points_] = polygon_points[vi]; + num_clipped_points_++; + } + } + else + { + if(prev_classify == 0) + { + if(num_clipped_points_ < getMaxTriangleClips()) + { + Vector3 tmp; + clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); + if(num_clipped_points_ > 0) + { + if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + else + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + } + } + + prev_classify = classify; + } + + if(num_clipped_points_ > 2) + { + if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < getEpsilon()) + { + num_clipped_points_--; + } + } + + *num_clipped_points = num_clipped_points_; +} + +//============================================================================== +template +void Intersect::clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point) +{ + S dist1 = distanceToPlane(n, t, v1); + Vector3 tmp = v2 - v1; + S dist2 = tmp.dot(n); + *clipped_point = tmp * (-dist1 / dist2) + v1; +} + +//============================================================================== +template +S Intersect::distanceToPlane(const Vector3& n, S t, const Vector3& v) +{ + return n.dot(v) - t; +} + +//============================================================================== +template +bool Intersect::buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t) +{ + Vector3 n_ = (v2 - v1).cross(v3 - v1); + bool can_normalize = false; + normalize(n_, &can_normalize); + if(can_normalize) + { + *n = n_; + *t = n_.dot(v1); + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t) +{ + Vector3 n_ = (v2 - v1).cross(tn); + bool can_normalize = false; + normalize(n_, &can_normalize); + if(can_normalize) + { + *n = n_; + *t = n_.dot(v1); + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t) +{ + S dist1 = distanceToPlane(n, t, v1); + S dist2 = dist1 * distanceToPlane(n, t, v2); + S dist3 = dist1 * distanceToPlane(n, t, v3); + if((dist2 > 0) && (dist3 > 0)) + return true; + return false; +} + +//============================================================================== +template +int Intersect::project6(const Vector3& ax, + const Vector3& p1, const Vector3& p2, const Vector3& p3, + const Vector3& q1, const Vector3& q2, const Vector3& q3) +{ + S P1 = ax.dot(p1); + S P2 = ax.dot(p2); + S P3 = ax.dot(p3); + S Q1 = ax.dot(q1); + S Q2 = ax.dot(q2); + S Q3 = ax.dot(q3); + + S mn1 = std::min(P1, std::min(P2, P3)); + S mx2 = std::max(Q1, std::max(Q2, Q3)); + if(mn1 > mx2) return 0; + + S mx1 = std::max(P1, std::max(P2, P3)); + S mn2 = std::min(Q1, std::min(Q2, Q3)); + + if(mn2 > mx1) return 0; + return 1; +} + +//============================================================================== +template +S Intersect::gaussianCDF(S x) +{ + return 0.5 * std::erfc(-x / sqrt(2.0)); +} + +//============================================================================== +template +constexpr S Intersect::getEpsilon() +{ + return 1e-5; +} + +//============================================================================== +template +constexpr S Intersect::getNearZeroThreshold() +{ + return 1e-7; +} + +//============================================================================== +template +constexpr S Intersect::getCcdResolution() +{ + return 1e-7; +} + +//============================================================================== +template +constexpr unsigned int Intersect::getMaxTriangleClips() +{ + return 8; +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/traversal/collision/intersect.h b/include/fcl/narrowphase/detail/traversal/collision/intersect.h index df33a8cdc..355876eb6 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/intersect.h +++ b/include/fcl/narrowphase/detail/traversal/collision/intersect.h @@ -1,265 +1,265 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#ifndef FCL_NARROWPHASE_DETAIL_INTERSECT_H -#define FCL_NARROWPHASE_DETAIL_INTERSECT_H - -#include -#include "fcl/common/types.h" -#include "fcl/math/geometry.h" -#include "fcl/math/detail/polysolver.h" - -namespace fcl -{ - -namespace detail -{ - -/// @brief CCD intersect kernel among primitives -template -class Intersect -{ - -public: - - /// @brief CCD intersect between one vertex and one face - /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 - /// p0 and p1 are points for vertex in time t0 and t1 - /// p_i returns the coordinate of the collision point - static bool intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between two edges - /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 - /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 - /// p_i returns the coordinate of the collision point - static bool intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between one vertex and one face, using additional filter - static bool intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between two edges, using additional filter - static bool intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between one vertex and and one edge - static bool intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& p1, - const Vector3& L); - - /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] - static bool intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - - /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] - static bool intersect_Triangle_ODE_style( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - - static bool intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Matrix3& R, - const Vector3& T, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - - static bool intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Transform3& tf, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - -private: - - /// @brief Project function used in intersect_Triangle() - static int project6(const Vector3& ax, - const Vector3& p1, const Vector3& p2, const Vector3& p3, - const Vector3& q1, const Vector3& q2, const Vector3& q3); - - /// @brief Check whether one value is zero - static bool isZero(S v); - - /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction - static bool solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S& l, S& r, bool bVF, S coeffs[], Vector3* data = nullptr); - - /// @brief Check whether one point p is within triangle [a, b, c] - static bool insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p); - - /// @brief Check whether one point p is within a line segment [a, b] - static bool insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p); - - /// @brief Calculate the line segment papb that is the shortest route between - /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where - /// pa = p1 + mua (p2 - p1) - /// pb = p3 + mub (p4 - p3) - /// return FALSE if no solution exists. - static bool linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, - Vector3* pa, Vector3* pb, S* mua, S* mub); - - /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t - static bool checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S t); - - /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time - static bool checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S t, Vector3* q_i = nullptr); - - /// @brief Check whether a root for VE intersection is valid - static bool checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - S t); - - /// @brief Solve a square function for EE intersection (with interval restriction) - static bool solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - bool bVF, - S* ret); - - /// @brief Solve a square function for VE intersection (with interval restriction) - static bool solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp); - - /// @brief Compute the cubic coefficients for VF intersection - /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. - - static void computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S* a, S* b, S* c, S* d); - - /// @brief Compute the cubic coefficients for EE intersection - static void computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S* a, S* b, S* c, S* d); - - /// @brief Compute the cubic coefficients for VE intersection - static void computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - const Vector3& L, - S* a, S* b, S* c); - - /// @brief filter for intersection, works for both VF and EE - static bool intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1); - - /// @brief distance of point v to a plane n * x - t = 0 - static S distanceToPlane(const Vector3& n, S t, const Vector3& v); - - /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 - static bool sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t); - - /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to - static void clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, - const Vector3& t1, const Vector3& t2, const Vector3& t3, - const Vector3& tn, S to, - Vector3 clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false); - - /// @brief build a plane passed through triangle v1 v2 v3 - static bool buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t); - - /// @brief build a plane pass through edge v1 and v2, normal is tn - static bool buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t); - - /// @brief compute the points which has deepest penetration depth - static void computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points); - - /// @brief clip polygon by plane - static void clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points); - - /// @brief clip a line segment by plane - static void clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point); - - /// @brief compute the cdf(x) - static S gaussianCDF(S x); - - static constexpr S getEpsilon(); - static constexpr S getNearZeroThreshold(); - static constexpr S getCcdResolution(); - static constexpr unsigned int getMaxTriangleClips(); -}; - -using Intersectf = Intersect; -using Intersectd = Intersect; - -} // namespace detail -} // namespace fcl - -#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#ifndef FCL_NARROWPHASE_DETAIL_INTERSECT_H +#define FCL_NARROWPHASE_DETAIL_INTERSECT_H + +#include +#include "fcl/common/types.h" +#include "fcl/math/geometry.h" +#include "fcl/math/detail/polysolver.h" + +namespace fcl +{ + +namespace detail +{ + +/// @brief CCD intersect kernel among primitives +template +class Intersect +{ + +public: + + /// @brief CCD intersect between one vertex and one face + /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 + /// p0 and p1 are points for vertex in time t0 and t1 + /// p_i returns the coordinate of the collision point + static bool intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between two edges + /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 + /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 + /// p_i returns the coordinate of the collision point + static bool intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between one vertex and one face, using additional filter + static bool intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between two edges, using additional filter + static bool intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between one vertex and and one edge + static bool intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& p1, + const Vector3& L); + + /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] + static bool intersect_Triangle_ODE_style( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Matrix3& R, + const Vector3& T, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Transform3& tf, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + +private: + + /// @brief Project function used in intersect_Triangle() + static int project6(const Vector3& ax, + const Vector3& p1, const Vector3& p2, const Vector3& p3, + const Vector3& q1, const Vector3& q2, const Vector3& q3); + + /// @brief Check whether one value is zero + static bool isZero(S v); + + /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction + static bool solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S& l, S& r, bool bVF, S coeffs[], Vector3* data = nullptr); + + /// @brief Check whether one point p is within triangle [a, b, c] + static bool insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p); + + /// @brief Check whether one point p is within a line segment [a, b] + static bool insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p); + + /// @brief Calculate the line segment papb that is the shortest route between + /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where + /// pa = p1 + mua (p2 - p1) + /// pb = p3 + mub (p4 - p3) + /// return FALSE if no solution exists. + static bool linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, + Vector3* pa, Vector3* pb, S* mua, S* mub); + + /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t + static bool checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S t); + + /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time + static bool checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S t, Vector3* q_i = nullptr); + + /// @brief Check whether a root for VE intersection is valid + static bool checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + S t); + + /// @brief Solve a square function for EE intersection (with interval restriction) + static bool solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + bool bVF, + S* ret); + + /// @brief Solve a square function for VE intersection (with interval restriction) + static bool solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp); + + /// @brief Compute the cubic coefficients for VF intersection + /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. + + static void computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S* a, S* b, S* c, S* d); + + /// @brief Compute the cubic coefficients for EE intersection + static void computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S* a, S* b, S* c, S* d); + + /// @brief Compute the cubic coefficients for VE intersection + static void computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + const Vector3& L, + S* a, S* b, S* c); + + /// @brief filter for intersection, works for both VF and EE + static bool intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1); + + /// @brief distance of point v to a plane n * x - t = 0 + static S distanceToPlane(const Vector3& n, S t, const Vector3& v); + + /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 + static bool sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t); + + /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to + static void clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, + const Vector3& t1, const Vector3& t2, const Vector3& t3, + const Vector3& tn, S to, + Vector3 clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false); + + /// @brief build a plane passed through triangle v1 v2 v3 + static bool buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t); + + /// @brief build a plane pass through edge v1 and v2, normal is tn + static bool buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t); + + /// @brief compute the points which has deepest penetration depth + static void computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points); + + /// @brief clip polygon by plane + static void clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points); + + /// @brief clip a line segment by plane + static void clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point); + + /// @brief compute the cdf(x) + static S gaussianCDF(S x); + + static constexpr S getEpsilon(); + static constexpr S getNearZeroThreshold(); + static constexpr S getCcdResolution(); + static constexpr unsigned int getMaxTriangleClips(); +}; + +using Intersectf = Intersect; +using Intersectd = Intersect; + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" + +#endif diff --git a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h index 6d0ec97f5..b3f8d7567 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h @@ -179,7 +179,7 @@ class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode { public: MeshCollisionTraversalNodekIOS(); - + bool BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; @@ -210,7 +210,7 @@ class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode* vertices; Triangle* tri_indices; - + S cost_density; const NarrowPhaseSolver* nsolver; diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h index a1cbfcd68..3f8035a6a 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h @@ -69,9 +69,9 @@ class MeshConservativeAdvancementTraversalNode bool canStop(S c) const; mutable S min_distance; - + mutable Vector3 closest_p1, closest_p2; - + mutable int last_tri_id1, last_tri_id2; /// @brief CA controlling variable: early stop for the early iterations of CA diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h index acd6a2a7e..894d95683 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h @@ -139,7 +139,7 @@ class MeshDistanceTraversalNodekIOS MeshDistanceTraversalNodekIOS(); void preprocess(); - + void postprocess(); S BVTesting(int b1, int b2) const diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h index 20b7e232b..68b97ec7b 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h @@ -72,7 +72,7 @@ class MeshShapeConservativeAdvancementTraversalNode mutable Vector3 closest_p1, closest_p2; mutable int last_tri_id; - + /// @brief CA controlling variable: early stop for the early iterations of CA S w; diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h index 79c5fde91..058a5c8a3 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h @@ -51,7 +51,7 @@ namespace detail template class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode -{ +{ public: using S = typename BV::S; @@ -69,7 +69,7 @@ class MeshShapeDistanceTraversalNode S rel_err; S abs_err; - + const NarrowPhaseSolver* nsolver; }; @@ -192,7 +192,7 @@ class MeshShapeDistanceTraversalNodeOBBRSS S BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; - + }; /// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h index 05ae2c10b..3d176a089 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h @@ -74,7 +74,7 @@ class ShapeBVHDistanceTraversalNode const Shape* model1; const BVHModel* model2; BV model1_bv; - + mutable int num_bv_tests; mutable int num_leaf_tests; mutable S query_time_seconds; diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h index 01b599527..069491bb7 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h @@ -72,7 +72,7 @@ class ShapeMeshConservativeAdvancementTraversalNode mutable Vector3 closest_p1, closest_p2; mutable int last_tri_id; - + /// @brief CA controlling variable: early stop for the early iterations of CA S w; diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h index a90c0077f..1777b8a8b 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h @@ -51,7 +51,7 @@ namespace detail template class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode -{ +{ public: using S = typename BV::S; @@ -69,7 +69,7 @@ class ShapeMeshDistanceTraversalNode S rel_err; S abs_err; - + const NarrowPhaseSolver* nsolver; }; @@ -140,7 +140,7 @@ class ShapeMeshDistanceTraversalNodekIOS S BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; - + }; /// @brief Initialize traversal node for distance computation between one shape @@ -174,7 +174,7 @@ class ShapeMeshDistanceTraversalNodeOBBRSS S BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; - + }; /// @brief Initialize traversal node for distance computation between one shape diff --git a/include/fcl/narrowphase/detail/traversal/traversal_node_base.h b/include/fcl/narrowphase/detail/traversal/traversal_node_base.h index 93cd7f096..abcc4ebdf 100644 --- a/include/fcl/narrowphase/detail/traversal/traversal_node_base.h +++ b/include/fcl/narrowphase/detail/traversal/traversal_node_base.h @@ -54,10 +54,10 @@ class TraversalNodeBase virtual ~TraversalNodeBase(); virtual void preprocess(); - + virtual void postprocess(); - /// @brief Whether b is a leaf node in the first BVH tree + /// @brief Whether b is a leaf node in the first BVH tree virtual bool isFirstNodeLeaf(int b) const; /// @brief Whether b is a leaf node in the second BVH tree diff --git a/include/fcl/narrowphase/distance.h b/include/fcl/narrowphase/distance.h index 9d77e52d0..cda024fe5 100644 --- a/include/fcl/narrowphase/distance.h +++ b/include/fcl/narrowphase/distance.h @@ -46,7 +46,7 @@ namespace fcl { -/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. +/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. /// Return value is the minimum distance generated between the two objects. template S distance( diff --git a/include/fcl/narrowphase/distance_result.h b/include/fcl/narrowphase/distance_result.h index 4430889b2..66ec7c26d 100644 --- a/include/fcl/narrowphase/distance_result.h +++ b/include/fcl/narrowphase/distance_result.h @@ -90,7 +90,7 @@ struct DistanceResult /// @brief invalid contact primitive information static const int NONE = -1; - + DistanceResult(S min_distance_ = std::numeric_limits::max()); /// @brief add distance information into the result diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index a80060abf..81df6ad42 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index 60fe06e06..d1975d16e 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index 3ff81662d..c86bed19b 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp index 8f3b24335..1afb67e69 100644 --- a/src/broadphase/broadphase_collision_manager.cpp +++ b/src/broadphase/broadphase_collision_manager.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/broadphase/broadphase_continuous_collision_manager.cpp b/src/broadphase/broadphase_continuous_collision_manager.cpp index 7084d769e..e30121133 100644 --- a/src/broadphase/broadphase_continuous_collision_manager.cpp +++ b/src/broadphase/broadphase_continuous_collision_manager.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index e43bcdb9d..2d51012c0 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 375c78b1d..8b9b065f0 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index 10c0cf368..70e527b94 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/broadphase/broadphase_spatialhash.cpp b/src/broadphase/broadphase_spatialhash.cpp index 0592102b4..d1fa84f09 100644 --- a/src/broadphase/broadphase_spatialhash.cpp +++ b/src/broadphase/broadphase_spatialhash.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp index 19450b399..11a67b3f4 100644 --- a/src/broadphase/detail/spatial_hash.cpp +++ b/src/broadphase/detail/spatial_hash.cpp @@ -31,7 +31,7 @@ * 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 Jia Pan */ diff --git a/src/geometry/octree/octree.cpp b/src/geometry/octree/octree.cpp index f94622fae..2e27cbb5e 100644 --- a/src/geometry/octree/octree.cpp +++ b/src/geometry/octree/octree.cpp @@ -55,4 +55,4 @@ void computeChildBV(const AABB& root_bv, unsigned int i, AABB& c } // namespace fcl -#endif +#endif \ No newline at end of file diff --git a/src/math/bv/kIOS.cpp b/src/math/bv/kIOS.cpp index 8c62ab7c3..b43a3e5e4 100644 --- a/src/math/bv/kIOS.cpp +++ b/src/math/bv/kIOS.cpp @@ -39,7 +39,7 @@ namespace fcl { - + template class FCL_INSTANTIATION_DEF_API kIOS; diff --git a/src/math/detail/polysolver.cpp b/src/math/detail/polysolver.cpp index 222063407..ed2f1ce78 100644 --- a/src/math/detail/polysolver.cpp +++ b/src/math/detail/polysolver.cpp @@ -1,49 +1,49 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#include "fcl/math/detail/polysolver-inl.h" - -namespace fcl -{ - -namespace detail { - -template -class FCL_INSTANTIATION_DEF_API PolySolver; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#include "fcl/math/detail/polysolver-inl.h" + +namespace fcl +{ + +namespace detail { + +template +class FCL_INSTANTIATION_DEF_API PolySolver; + +} // namespace detail +} // namespace fcl diff --git a/src/math/detail/project.cpp b/src/math/detail/project.cpp index b26fecddf..9024d2efb 100644 --- a/src/math/detail/project.cpp +++ b/src/math/detail/project.cpp @@ -1,50 +1,50 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#include "fcl/math/detail/project-inl.h" - -namespace fcl -{ - -namespace detail -{ - -template -class FCL_INSTANTIATION_DEF_API Project; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#include "fcl/math/detail/project-inl.h" + +namespace fcl +{ + +namespace detail +{ + +template +class FCL_INSTANTIATION_DEF_API Project; + +} // namespace detail +} // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp b/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp index c0ef3efda..094def0e0 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp @@ -1,50 +1,50 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" - -namespace fcl -{ - -namespace detail -{ - -template -class FCL_INSTANTIATION_DEF_API Intersect; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" + +namespace fcl +{ + +namespace detail +{ + +template +class FCL_INSTANTIATION_DEF_API Intersect; + +} // namespace detail +} // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp b/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp index 62c14a1dc..abaeb602a 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp @@ -1,51 +1,51 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, Open Source Robotics Foundation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Open Source Robotics Foundation nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/** @author Jia Pan */ - -#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -template -class FCL_INSTANTIATION_DEF_API TriangleDistance; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Jia Pan */ + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +template +class FCL_INSTANTIATION_DEF_API TriangleDistance; + +} // namespace detail +} // namespace fcl From 76a089b47678de068d571c65416688f7ef6976e6 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Sun, 30 Jan 2022 11:18:40 +0100 Subject: [PATCH 14/16] add missing explicit instantiation declaration --- include/fcl/geometry/shape/utility-inl.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/include/fcl/geometry/shape/utility-inl.h b/include/fcl/geometry/shape/utility-inl.h index 7b0eb799c..6dd23242f 100644 --- a/include/fcl/geometry/shape/utility-inl.h +++ b/include/fcl/geometry/shape/utility-inl.h @@ -56,6 +56,11 @@ namespace fcl { +//============================================================================== +extern template +FCL_EXTERN_TEMPLATE_API +void constructBox(const AABB& bv, Box& box, Transform3& tf); + //============================================================================== extern template FCL_EXTERN_TEMPLATE_API From 883b5da349f71b7279d1a53ed9b703a021d9f6a8 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Sun, 30 Jan 2022 13:13:09 +0100 Subject: [PATCH 15/16] define templates after explicit instantiation declaration --- include/fcl/math/bv/utility-inl.h | 728 +++++++++++++++++------------- 1 file changed, 419 insertions(+), 309 deletions(-) diff --git a/include/fcl/math/bv/utility-inl.h b/include/fcl/math/bv/utility-inl.h index 32b1d0040..f933e8460 100644 --- a/include/fcl/math/bv/utility-inl.h +++ b/include/fcl/math/bv/utility-inl.h @@ -60,6 +60,46 @@ namespace detail { namespace OBB_fit_functions { //============================================================================== +//============================================================================== +template +void fit1(const Vector3* ps, OBB& bv); + +extern template +FCL_EXTERN_TEMPLATE_API +void fit1(const Vector3d* ps, OBB& bv); + +//============================================================================== +template +void fit2(const Vector3* ps, OBB& bv); + +extern template +FCL_EXTERN_TEMPLATE_API +void fit2(const Vector3d* ps, OBB& bv); + +//============================================================================== +template +void fit3(const Vector3* ps, OBB& bv); + +extern template +FCL_EXTERN_TEMPLATE_API +void fit3(const Vector3d* ps, OBB& bv); + +//============================================================================== +template +void fit6(const Vector3* ps, OBB& bv); + +extern template +FCL_EXTERN_TEMPLATE_API +void fit6(const Vector3d* ps, OBB& bv); + +//============================================================================== +template +void fitn(const Vector3* ps, int n, OBB& bv); + +extern template +FCL_EXTERN_TEMPLATE_API +void fitn(const Vector3d* ps, int n, OBB& bv); + //============================================================================== template void fit1(const Vector3* const ps, OBB& bv) @@ -140,37 +180,52 @@ void fitn(const Vector3* const ps, int n, OBB& bv) } //============================================================================== -extern template -FCL_EXTERN_TEMPLATE_API -void fit1(const Vector3d* const ps, OBB& bv); +} // namespace OBB_fit_functions +//============================================================================== + +//============================================================================== +namespace RSS_fit_functions { +//============================================================================== //============================================================================== +template +void fit1(const Vector3* ps, RSS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fit2(const Vector3d* const ps, OBB& bv); +void fit1(const Vector3d* ps, RSS& bv); //============================================================================== +template +void fit2(const Vector3* ps, RSS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fit3(const Vector3d* const ps, OBB& bv); +void fit2(const Vector3d* ps, RSS& bv); //============================================================================== +template +void fit3(const Vector3* ps, RSS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fit6(const Vector3d* const ps, OBB& bv); +void fit3(const Vector3d* ps, RSS& bv); //============================================================================== +template +void fit6(const Vector3* ps, RSS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fitn(const Vector3d* const ps, int n, OBB& bv); +void fit6(const Vector3d* ps, RSS& bv); //============================================================================== -} // namespace OBB_fit_functions -//============================================================================== +template +void fitn(const Vector3* ps, int n, RSS& bv); -//============================================================================== -namespace RSS_fit_functions { -//============================================================================== +extern template +FCL_EXTERN_TEMPLATE_API +void fitn(const Vector3d* ps, int n, RSS& bv); //============================================================================== template @@ -254,37 +309,44 @@ void fitn(const Vector3* const ps, int n, RSS& bv) } //============================================================================== -extern template -FCL_EXTERN_TEMPLATE_API -void fit1(const Vector3d* const ps, RSS& bv); +} // namespace RSS_fit_functions +//============================================================================== //============================================================================== -extern template -FCL_EXTERN_TEMPLATE_API -void fit2(const Vector3d* const ps, RSS& bv); +namespace kIOS_fit_functions { +//============================================================================== //============================================================================== +template +void fit1(const Vector3* ps, kIOS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fit3(const Vector3d* const ps, RSS& bv); +void fit1(const Vector3d* ps, kIOS& bv); //============================================================================== +template +void fit2(const Vector3* ps, kIOS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fit6(const Vector3d* const ps, RSS& bv); +void fit2(const Vector3d* ps, kIOS& bv); //============================================================================== +template +void fit3(const Vector3* ps, kIOS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fitn(const Vector3d* const ps, int n, RSS& bv); +void fit3(const Vector3d* ps, kIOS& bv); //============================================================================== -} // namespace RSS_fit_functions -//============================================================================== +template +void fitn(const Vector3* ps, int n, kIOS& bv); -//============================================================================== -namespace kIOS_fit_functions { -//============================================================================== +extern template +FCL_EXTERN_TEMPLATE_API +void fitn(const Vector3d* ps, int n, kIOS& bv); //============================================================================== template @@ -447,32 +509,44 @@ void fitn(const Vector3* const ps, int n, kIOS& bv) } //============================================================================== -extern template -FCL_EXTERN_TEMPLATE_API -void fit1(const Vector3d* const ps, kIOS& bv); +} // namespace kIOS_fit_functions +//============================================================================== + +//============================================================================== +namespace OBBRSS_fit_functions { +//============================================================================== //============================================================================== +template +void fit1(const Vector3* ps, OBBRSS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fit2(const Vector3d* const ps, kIOS& bv); +void fit1(const Vector3d* ps, OBBRSS& bv); //============================================================================== +template +void fit2(const Vector3* ps, OBBRSS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fit3(const Vector3d* const ps, kIOS& bv); +void fit2(const Vector3d* ps, OBBRSS& bv); //============================================================================== +template +void fit3(const Vector3* ps, OBBRSS& bv); + extern template FCL_EXTERN_TEMPLATE_API -void fitn(const Vector3d* const ps, int n, kIOS& bv); +void fit3(const Vector3d* ps, OBBRSS& bv); //============================================================================== -} // namespace kIOS_fit_functions -//============================================================================== +template +void fitn(const Vector3* ps, int n, OBBRSS& bv); -//============================================================================== -namespace OBBRSS_fit_functions { -//============================================================================== +extern template +FCL_EXTERN_TEMPLATE_API +void fitn(const Vector3d* ps, int n, OBBRSS& bv); //============================================================================== template @@ -506,26 +580,6 @@ void fitn(const Vector3* const ps, int n, OBBRSS& bv) RSS_fit_functions::fitn(ps, n, bv.rss); } -//============================================================================== -extern template -FCL_EXTERN_TEMPLATE_API -void fit1(const Vector3d* const ps, OBBRSS& bv); - -//============================================================================== -extern template -FCL_EXTERN_TEMPLATE_API -void fit2(const Vector3d* const ps, OBBRSS& bv); - -//============================================================================== -extern template -FCL_EXTERN_TEMPLATE_API -void fit3(const Vector3d* const ps, OBBRSS& bv); - -//============================================================================== -extern template -FCL_EXTERN_TEMPLATE_API -void fitn(const Vector3d* const ps, int n, OBBRSS& bv); - //============================================================================== } // namespace OBBRSS_fit_functions //============================================================================== @@ -534,123 +588,139 @@ void fitn(const Vector3d* const ps, int n, OBBRSS& bv); template struct Fitter { - static void fit(const Vector3* const ps, int n, BV& bv) - { - for(int i = 0; i < n; ++i) - bv += ps[i]; - } + static void fit(const Vector3* ps, int n, BV& bv); }; //============================================================================== template struct Fitter> { - static void fit(const Vector3* const ps, int n, OBB& bv) - { - switch(n) - { - case 1: - OBB_fit_functions::fit1(ps, bv); - break; - case 2: - OBB_fit_functions::fit2(ps, bv); - break; - case 3: - OBB_fit_functions::fit3(ps, bv); - break; - case 6: - OBB_fit_functions::fit6(ps, bv); - break; - default: - OBB_fit_functions::fitn(ps, n, bv); - } - } + static void fit(const Vector3* ps, int n, OBB& bv); }; +extern template +struct FCL_EXTERN_TEMPLATE_API Fitter>; + //============================================================================== template struct Fitter> { - static void fit(const Vector3* const ps, int n, RSS& bv) - { - switch(n) - { - case 1: - RSS_fit_functions::fit1(ps, bv); - break; - case 2: - RSS_fit_functions::fit2(ps, bv); - break; - case 3: - RSS_fit_functions::fit3(ps, bv); - break; - default: - RSS_fit_functions::fitn(ps, n, bv); - } - } + static void fit(const Vector3* ps, int n, RSS& bv); }; +extern template +struct FCL_EXTERN_TEMPLATE_API Fitter>; + //============================================================================== template struct Fitter> { - static void fit(const Vector3* const ps, int n, kIOS& bv) - { - switch(n) - { - case 1: - kIOS_fit_functions::fit1(ps, bv); - break; - case 2: - kIOS_fit_functions::fit2(ps, bv); - break; - case 3: - kIOS_fit_functions::fit3(ps, bv); - break; - default: - kIOS_fit_functions::fitn(ps, n, bv); - } - } + static void fit(const Vector3* ps, int n, kIOS& bv); }; +extern template +struct FCL_EXTERN_TEMPLATE_API Fitter>; + //============================================================================== template struct Fitter> { - static void fit(const Vector3* const ps, int n, OBBRSS& bv) - { - switch(n) - { - case 1: - OBBRSS_fit_functions::fit1(ps, bv); - break; - case 2: - OBBRSS_fit_functions::fit2(ps, bv); - break; - case 3: - OBBRSS_fit_functions::fit3(ps, bv); - break; - default: - OBBRSS_fit_functions::fitn(ps, n, bv); - } - } + static void fit(const Vector3* ps, int n, OBBRSS& bv); }; -//============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API Fitter>; +struct FCL_EXTERN_TEMPLATE_API Fitter>; //============================================================================== -extern template -struct FCL_EXTERN_TEMPLATE_API Fitter>; +template +void Fitter::fit(const Vector3* const ps, int n, BV& bv) +{ + for(int i = 0; i < n; ++i) + bv += ps[i]; +} //============================================================================== -extern template -struct FCL_EXTERN_TEMPLATE_API Fitter>; +template +void Fitter>::fit(const Vector3* const ps, int n, OBB& bv) +{ + switch(n) + { + case 1: + OBB_fit_functions::fit1(ps, bv); + break; + case 2: + OBB_fit_functions::fit2(ps, bv); + break; + case 3: + OBB_fit_functions::fit3(ps, bv); + break; + case 6: + OBB_fit_functions::fit6(ps, bv); + break; + default: + OBB_fit_functions::fitn(ps, n, bv); + } +} //============================================================================== -extern template -struct FCL_EXTERN_TEMPLATE_API Fitter>; +template +void Fitter>::fit(const Vector3* const ps, int n, RSS& bv) +{ + switch(n) + { + case 1: + RSS_fit_functions::fit1(ps, bv); + break; + case 2: + RSS_fit_functions::fit2(ps, bv); + break; + case 3: + RSS_fit_functions::fit3(ps, bv); + break; + default: + RSS_fit_functions::fitn(ps, n, bv); + } +} + +//============================================================================== +template +void Fitter>::fit(const Vector3* const ps, int n, kIOS& bv) +{ + switch(n) + { + case 1: + kIOS_fit_functions::fit1(ps, bv); + break; + case 2: + kIOS_fit_functions::fit2(ps, bv); + break; + case 3: + kIOS_fit_functions::fit3(ps, bv); + break; + default: + kIOS_fit_functions::fitn(ps, n, bv); + } +} + +//============================================================================== +template +void Fitter>::fit(const Vector3* const ps, int n, OBBRSS& bv) +{ + switch(n) + { + case 1: + OBBRSS_fit_functions::fit1(ps, bv); + break; + case 2: + OBBRSS_fit_functions::fit2(ps, bv); + break; + case 3: + OBBRSS_fit_functions::fit3(ps, bv); + break; + default: + OBBRSS_fit_functions::fitn(ps, n, bv); + } +} //============================================================================== } // namespace detail @@ -667,20 +737,22 @@ void fit(const Vector3* const ps, int n, BV& bv) namespace detail { //============================================================================== +//============================================================================== /// @brief Convert a bounding volume of type BV1 in configuration tf1 to a /// bounding volume of type BV2 in I configuration. template class ConvertBVImpl { private: - static void run(const BV1& bv1, const Transform3& tf1, BV2& bv2) - { - FCL_UNUSED(bv1); - FCL_UNUSED(tf1); - FCL_UNUSED(bv2); + static void run(const BV1& bv1, const Transform3& tf1, BV2& bv2); +}; - // should only use the specialized version, so it is private. - } +//============================================================================== +template +class ConvertBVImpl> +{ +public: + static void run(const BV1& bv1, const Transform3& tf1, AABB& bv2); }; //============================================================================== @@ -689,15 +761,18 @@ template class ConvertBVImpl, AABB> { public: - static void run(const AABB& bv1, const Transform3& tf1, AABB& bv2) - { - const Vector3 center = bv1.center(); - S r = (bv1.max_ - bv1.min_).norm() * 0.5; - Vector3 center2 = tf1 * center; - Vector3 delta(r, r, r); - bv2.min_ = center2 - delta; - bv2.max_ = center2 + delta; - } + static void run(const AABB& bv1, const Transform3& tf1, AABB& bv2); +}; + +extern template +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, AABB>; + +//============================================================================== +template +class ConvertBVImpl> +{ +public: + static void run(const BV1& bv1, const Transform3& tf1, OBB& bv2); }; //============================================================================== @@ -705,177 +780,140 @@ template class ConvertBVImpl, OBB> { public: - static void run(const AABB& bv1, const Transform3& tf1, OBB& bv2) - { - /* - bv2.To = tf1 * bv1.center()); - - /// Sort the AABB edges so that AABB extents are ordered. - S d[3] = {bv1.width(), bv1.height(), bv1.depth() }; - std::size_t id[3] = {0, 1, 2}; - - for(std::size_t i = 1; i < 3; ++i) - { - for(std::size_t j = i; j > 0; --j) - { - if(d[j] > d[j-1]) - { - { - S tmp = d[j]; - d[j] = d[j-1]; - d[j-1] = tmp; - } - { - std::size_t tmp = id[j]; - id[j] = id[j-1]; - id[j-1] = tmp; - } - } - } - } - - Vector3 extent = (bv1.max_ - bv1.min_) * 0.5; - bv2.extent = Vector3(extent[id[0]], extent[id[1]], extent[id[2]]); - const Matrix3& R = tf1.linear(); - bool left_hand = (id[0] == (id[1] + 1) % 3); - bv2.axis[0] = left_hand ? -R.col(id[0]) : R.col(id[0]); - bv2.axis[1] = R.col(id[1]); - bv2.axis[2] = R.col(id[2]); - */ - - bv2.To.noalias() = tf1 * bv1.center(); - bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; - bv2.axis = tf1.linear(); - } + static void run(const AABB& bv1, const Transform3& tf1, OBB& bv2); }; +extern template +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; + //============================================================================== template class ConvertBVImpl, OBB> { public: - static void run(const OBB& bv1, const Transform3& tf1, OBB& bv2) - { - bv2.extent = bv1.extent; - bv2.To.noalias() = tf1 * bv1.To; - bv2.axis.noalias() = tf1.linear() * bv1.axis; - } + static void run(const OBB& bv1, const Transform3& tf1, OBB& bv2); }; +extern template +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; + //============================================================================== template class ConvertBVImpl, OBB> { public: - static void run(const OBBRSS& bv1, const Transform3& tf1, OBB& bv2) - { - ConvertBVImpl, OBB>::run(bv1.obb, tf1, bv2); - } + static void run(const OBBRSS& bv1, const Transform3& tf1, OBB& bv2); }; +extern template +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; + //============================================================================== template class ConvertBVImpl, OBB> { public: - static void run(const RSS& bv1, const Transform3& tf1, OBB& bv2) - { - bv2.extent << bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r; - bv2.To.noalias() = tf1 * bv1.center(); - bv2.axis.noalias() = tf1.linear() * bv1.axis; - } + static void run(const RSS& bv1, const Transform3& tf1, OBB& bv2); }; +extern template +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; + //============================================================================== -template -class ConvertBVImpl> +template +class ConvertBVImpl, RSS> { public: - static void run(const BV1& bv1, const Transform3& tf1, AABB& bv2) - { - const Vector3 center = bv1.center(); - S r = Vector3(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; - Vector3 delta(r, r, r); - Vector3 center2 = tf1 * center; - bv2.min_ = center2 - delta; - bv2.max_ = center2 + delta; - } + static void run(const OBB& bv1, const Transform3& tf1, RSS& bv2); }; +extern template +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; + //============================================================================== -template -class ConvertBVImpl> +template +class ConvertBVImpl, RSS> { public: - static void run(const BV1& bv1, const Transform3& tf1, OBB& bv2) - { - AABB bv; - ConvertBVImpl>::run(bv1, Transform3::Identity(), bv); - ConvertBVImpl, OBB>::run(bv, tf1, bv2); - } + static void run(const RSS& bv1, const Transform3& tf1, RSS& bv2); }; +extern template +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; + //============================================================================== template -class ConvertBVImpl, RSS> +class ConvertBVImpl, RSS> { public: - static void run(const OBB& bv1, const Transform3& tf1, RSS& bv2) - { - // OBB's rotation matrix in axis is required to be lined up with the - // x-axis along the longest edge, the y-axis on the next longest edge - // and the z-axis on the shortest edge. RSS requires the longest edge - // of the rectangle to be the x-axis and the next longest the y-axis. - // This maps perfectly from OBB to RSS so simply transform the rotation - // axis of the OBB into the RSS. - bv2.axis.noalias() = tf1.linear() * bv1.axis; - - // Set longest rectangle side for RSS to longest dimension of OBB. - bv2.l[0] = 2 * (bv1.extent[0]); - // Set shortest rectangle side for RSS to next-longest dimension of OBB. - bv2.l[1] = 2 * (bv1.extent[1]); - // Set radius for RSS to the smallest dimension of OBB. - bv2.r = bv1.extent[2]; - - // OBB's To is at its center while RSS's To is at a corner. - bv2.setToFromCenter(tf1 * bv1.center()); - } + static void run(const OBBRSS& bv1, const Transform3& tf1, RSS& bv2); }; +extern template +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; + //============================================================================== template -class ConvertBVImpl, RSS> +class ConvertBVImpl, RSS> { public: - static void run(const RSS& bv1, const Transform3& tf1, RSS& bv2) - { - bv2.To.noalias() = tf1 * bv1.To; - bv2.axis.noalias() = tf1.linear() * bv1.axis; - - bv2.r = bv1.r; - bv2.l[0] = bv1.l[0]; - bv2.l[1] = bv1.l[1]; - } + static void run(const AABB& bv1, const Transform3& tf1, RSS& bv2); }; +extern template +class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; + +//============================================================================== +template +void ConvertBVImpl::run(const BV1& bv1, const Transform3& tf1, BV2& bv2) +{ + FCL_UNUSED(bv1); + FCL_UNUSED(tf1); + FCL_UNUSED(bv2); + + // should only use the specialized version, so it is private. +} + +//============================================================================== +template +void ConvertBVImpl>::run(const BV1& bv1, const Transform3& tf1, AABB& bv2) +{ + const Vector3 center = bv1.center(); + S r = Vector3(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5; + Vector3 delta(r, r, r); + Vector3 center2 = tf1 * center; + bv2.min_ = center2 - delta; + bv2.max_ = center2 + delta; +} + //============================================================================== template -class ConvertBVImpl, RSS> +void ConvertBVImpl, AABB>::run(const AABB& bv1, const Transform3& tf1, AABB& bv2) { -public: - static void run(const OBBRSS& bv1, const Transform3& tf1, RSS& bv2) - { - ConvertBVImpl, RSS>::run(bv1.rss, tf1, bv2); - } -}; + const Vector3 center = bv1.center(); + S r = (bv1.max_ - bv1.min_).norm() * 0.5; + Vector3 center2 = tf1 * center; + Vector3 delta(r, r, r); + bv2.min_ = center2 - delta; + bv2.max_ = center2 + delta; +} + +//============================================================================== +template +void ConvertBVImpl>::run(const BV1& bv1, const Transform3& tf1, OBB& bv2) +{ + AABB bv; + ConvertBVImpl>::run(bv1, Transform3::Identity(), bv); + ConvertBVImpl, OBB>::run(bv, tf1, bv2); +} //============================================================================== template -class ConvertBVImpl, RSS> +void ConvertBVImpl, OBB>::run(const AABB& bv1, const Transform3& tf1, OBB& bv2) { -public: - static void run(const AABB& bv1, const Transform3& tf1, RSS& bv2) - { + /* + bv2.To = tf1 * bv1.center()); + /// Sort the AABB edges so that AABB extents are ordered. S d[3] = {bv1.width(), bv1.height(), bv1.depth() }; std::size_t id[3] = {0, 1, 2}; @@ -901,57 +939,129 @@ class ConvertBVImpl, RSS> } Vector3 extent = (bv1.max_ - bv1.min_) * 0.5; - bv2.r = extent[id[2]]; - bv2.l[0] = (extent[id[0]]) * 2; - bv2.l[1] = (extent[id[1]]) * 2; - + bv2.extent = Vector3(extent[id[0]], extent[id[1]], extent[id[2]]); const Matrix3& R = tf1.linear(); bool left_hand = (id[0] == (id[1] + 1) % 3); - if (left_hand) - bv2.axis.col(0) = -R.col(id[0]); - else - bv2.axis.col(0) = R.col(id[0]); - bv2.axis.col(1) = R.col(id[1]); - bv2.axis.col(2) = R.col(id[2]); - bv2.setToFromCenter(tf1 * bv1.center()); - } -}; + bv2.axis[0] = left_hand ? -R.col(id[0]) : R.col(id[0]); + bv2.axis[1] = R.col(id[1]); + bv2.axis[2] = R.col(id[2]); + */ + + bv2.To.noalias() = tf1 * bv1.center(); + bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; + bv2.axis = tf1.linear(); +} //============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, AABB>; +template +void ConvertBVImpl, OBB>::run(const OBB& bv1, const Transform3& tf1, OBB& bv2) +{ + bv2.extent = bv1.extent; + bv2.To.noalias() = tf1 * bv1.To; + bv2.axis.noalias() = tf1.linear() * bv1.axis; +} //============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; +template +void ConvertBVImpl, OBB>::run(const OBBRSS& bv1, const Transform3& tf1, OBB& bv2) +{ + ConvertBVImpl, OBB>::run(bv1.obb, tf1, bv2); +} //============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; +template +void ConvertBVImpl, OBB>::run(const RSS& bv1, const Transform3& tf1, OBB& bv2) +{ + bv2.extent << bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r; + bv2.To.noalias() = tf1 * bv1.center(); + bv2.axis.noalias() = tf1.linear() * bv1.axis; +} //============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; +template +void ConvertBVImpl, RSS>::run(const OBB& bv1, const Transform3& tf1, RSS& bv2) +{ + // OBB's rotation matrix in axis is required to be lined up with the + // x-axis along the longest edge, the y-axis on the next longest edge + // and the z-axis on the shortest edge. RSS requires the longest edge + // of the rectangle to be the x-axis and the next longest the y-axis. + // This maps perfectly from OBB to RSS so simply transform the rotation + // axis of the OBB into the RSS. + bv2.axis.noalias() = tf1.linear() * bv1.axis; + + // Set longest rectangle side for RSS to longest dimension of OBB. + bv2.l[0] = 2 * (bv1.extent[0]); + // Set shortest rectangle side for RSS to next-longest dimension of OBB. + bv2.l[1] = 2 * (bv1.extent[1]); + // Set radius for RSS to the smallest dimension of OBB. + bv2.r = bv1.extent[2]; + + // OBB's To is at its center while RSS's To is at a corner. + bv2.setToFromCenter(tf1 * bv1.center()); +} //============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; +template +void ConvertBVImpl, RSS>::run(const RSS& bv1, const Transform3& tf1, RSS& bv2) +{ + bv2.To.noalias() = tf1 * bv1.To; + bv2.axis.noalias() = tf1.linear() * bv1.axis; -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; + bv2.r = bv1.r; + bv2.l[0] = bv1.l[0]; + bv2.l[1] = bv1.l[1]; +} //============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; +template +void ConvertBVImpl, RSS>::run(const OBBRSS& bv1, const Transform3& tf1, RSS& bv2) +{ + ConvertBVImpl, RSS>::run(bv1.rss, tf1, bv2); +} //============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; +template +void ConvertBVImpl, RSS>::run(const AABB& bv1, const Transform3& tf1, RSS& bv2) +{ + /// Sort the AABB edges so that AABB extents are ordered. + S d[3] = {bv1.width(), bv1.height(), bv1.depth() }; + std::size_t id[3] = {0, 1, 2}; -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; + for(std::size_t i = 1; i < 3; ++i) + { + for(std::size_t j = i; j > 0; --j) + { + if(d[j] > d[j-1]) + { + { + S tmp = d[j]; + d[j] = d[j-1]; + d[j-1] = tmp; + } + { + std::size_t tmp = id[j]; + id[j] = id[j-1]; + id[j-1] = tmp; + } + } + } + } + + Vector3 extent = (bv1.max_ - bv1.min_) * 0.5; + bv2.r = extent[id[2]]; + bv2.l[0] = (extent[id[0]]) * 2; + bv2.l[1] = (extent[id[1]]) * 2; + + const Matrix3& R = tf1.linear(); + bool left_hand = (id[0] == (id[1] + 1) % 3); + if (left_hand) + bv2.axis.col(0) = -R.col(id[0]); + else + bv2.axis.col(0) = R.col(id[0]); + bv2.axis.col(1) = R.col(id[1]); + bv2.axis.col(2) = R.col(id[2]); + bv2.setToFromCenter(tf1 * bv1.center()); +} //============================================================================== } // namespace detail From fcecb853f7af08f97a91911893008fe02bd844a0 Mon Sep 17 00:00:00 2001 From: SpaceIm <30052553+SpaceIm@users.noreply.github.com> Date: Mon, 1 Aug 2022 13:30:19 +0200 Subject: [PATCH 16/16] better solution --- include/fcl/CMakeLists.txt | 16 +-- include/fcl/broadphase/broadphase_SSaP-inl.h | 4 +- include/fcl/broadphase/broadphase_SaP-inl.h | 4 +- .../broadphase/broadphase_bruteforce-inl.h | 4 +- .../broadphase_collision_manager-inl.h | 4 +- ...adphase_continuous_collision_manager-inl.h | 4 +- .../broadphase_dynamic_AABB_tree-inl.h | 4 +- .../broadphase_dynamic_AABB_tree_array-inl.h | 4 +- .../broadphase/broadphase_interval_tree-inl.h | 4 +- .../broadphase/broadphase_spatialhash-inl.h | 4 +- .../fcl/broadphase/detail/interval_tree-inl.h | 4 +- include/fcl/broadphase/detail/interval_tree.h | 4 +- .../detail/interval_tree_node-inl.h | 4 +- include/fcl/broadphase/detail/morton-inl.h | 8 +- include/fcl/broadphase/detail/morton.h | 4 +- .../broadphase/detail/simple_interval-inl.h | 4 +- .../fcl/broadphase/detail/spatial_hash-inl.h | 4 +- include/fcl/common/detail/profiler.h | 10 +- include/fcl/common/exception.h | 2 +- include/fcl/common/time.h | 6 +- include/fcl/geometry/bvh/BVH_utility-inl.h | 6 +- include/fcl/geometry/bvh/BV_node_base.h | 2 +- include/fcl/geometry/bvh/detail/BVH_front.h | 4 +- include/fcl/geometry/collision_geometry-inl.h | 4 +- include/fcl/geometry/octree/octree-inl.h | 6 +- include/fcl/geometry/shape/box-inl.h | 4 +- include/fcl/geometry/shape/capsule-inl.h | 4 +- include/fcl/geometry/shape/cone-inl.h | 4 +- include/fcl/geometry/shape/convex-inl.h | 4 +- include/fcl/geometry/shape/cylinder-inl.h | 4 +- include/fcl/geometry/shape/ellipsoid-inl.h | 4 +- include/fcl/geometry/shape/halfspace-inl.h | 6 +- include/fcl/geometry/shape/plane-inl.h | 6 +- include/fcl/geometry/shape/shape_base-inl.h | 4 +- include/fcl/geometry/shape/sphere-inl.h | 4 +- include/fcl/geometry/shape/triangle_p-inl.h | 4 +- include/fcl/geometry/shape/utility-inl.h | 94 ++++++------- include/fcl/math/bv/AABB-inl.h | 4 +- include/fcl/math/bv/OBB-inl.h | 14 +- include/fcl/math/bv/OBBRSS-inl.h | 6 +- include/fcl/math/bv/RSS-inl.h | 16 ++- include/fcl/math/bv/kDOP-inl.h | 18 +-- include/fcl/math/bv/kIOS-inl.h | 4 +- include/fcl/math/bv/utility-inl.h | 124 +++++++++++++----- include/fcl/math/detail/polysolver-inl.h | 8 +- include/fcl/math/detail/polysolver.h | 4 +- include/fcl/math/detail/project-inl.h | 8 +- include/fcl/math/detail/project.h | 4 +- include/fcl/math/detail/seed.h | 2 +- include/fcl/math/geometry-inl.h | 42 +++--- include/fcl/math/motion/interp_motion-inl.h | 4 +- include/fcl/math/motion/motion_base-inl.h | 4 +- include/fcl/math/motion/screw_motion-inl.h | 4 +- include/fcl/math/motion/spline_motion-inl.h | 4 +- .../math/motion/taylor_model/interval-inl.h | 8 +- .../motion/taylor_model/interval_matrix-inl.h | 6 +- .../motion/taylor_model/interval_vector-inl.h | 8 +- .../motion/taylor_model/taylor_matrix-inl.h | 18 +-- .../motion/taylor_model/taylor_model-inl.h | 16 ++- .../motion/taylor_model/taylor_vector-inl.h | 12 +- .../motion/taylor_model/time_interval-inl.h | 4 +- .../fcl/math/motion/translation_motion-inl.h | 4 +- .../triangle_motion_bound_visitor-inl.h | 4 +- include/fcl/math/rng-inl.h | 4 +- include/fcl/math/sampler/sampler_base.h | 4 +- include/fcl/math/sampler/sampler_se2-inl.h | 4 +- .../fcl/math/sampler/sampler_se2_disk-inl.h | 4 +- .../fcl/math/sampler/sampler_se3_euler-inl.h | 4 +- .../math/sampler/sampler_se3_euler_ball-inl.h | 4 +- .../fcl/math/sampler/sampler_se3_quat-inl.h | 4 +- .../math/sampler/sampler_se3_quat_ball-inl.h | 4 +- include/fcl/math/triangle.h | 2 +- include/fcl/math/variance3-inl.h | 4 +- include/fcl/narrowphase/collision-inl.h | 6 +- .../fcl/narrowphase/collision_object-inl.h | 4 +- .../fcl/narrowphase/collision_request-inl.h | 4 +- .../fcl/narrowphase/collision_result-inl.h | 4 +- include/fcl/narrowphase/contact-inl.h | 4 +- include/fcl/narrowphase/contact_point-inl.h | 8 +- .../narrowphase/continuous_collision-inl.h | 10 +- .../continuous_collision_object-inl.h | 4 +- .../continuous_collision_request-inl.h | 4 +- .../continuous_collision_result-inl.h | 4 +- include/fcl/narrowphase/cost_source-inl.h | 4 +- .../convexity_based_algorithm/epa-inl.h | 4 +- .../convexity_based_algorithm/gjk-inl.h | 4 +- .../gjk_libccd-inl.h | 26 ++-- .../convexity_based_algorithm/gjk_libccd.h | 6 +- .../minkowski_diff-inl.h | 4 +- .../detail/failed_at_this_configuration.h | 4 +- .../narrowphase/detail/gjk_solver_indep-inl.h | 4 +- .../detail/gjk_solver_libccd-inl.h | 4 +- .../primitive_shape_algorithm/box_box-inl.h | 12 +- .../capsule_capsule-inl.h | 8 +- .../primitive_shape_algorithm/halfspace-inl.h | 28 ++-- .../primitive_shape_algorithm/halfspace.h | 4 +- .../primitive_shape_algorithm/plane-inl.h | 24 ++-- .../detail/primitive_shape_algorithm/plane.h | 4 +- .../sphere_box-inl.h | 6 +- .../sphere_capsule-inl.h | 8 +- .../sphere_cylinder-inl.h | 6 +- .../sphere_sphere-inl.h | 6 +- .../sphere_triangle-inl.h | 14 +- .../triangle_distance-inl.h | 4 +- .../collision_traversal_node_base-inl.h | 4 +- .../traversal/collision/intersect-inl.h | 4 +- .../mesh_collision_traversal_node-inl.h | 18 +-- ..._continuous_collision_traversal_node-inl.h | 4 +- .../detail/traversal/collision_node-inl.h | 12 +- .../conservative_advancement_stack_data-inl.h | 4 +- .../distance_traversal_node_base-inl.h | 4 +- ...servative_advancement_traversal_node-inl.h | 10 +- .../mesh_distance_traversal_node-inl.h | 14 +- .../traversal/traversal_node_base-inl.h | 4 +- .../detail/traversal/traversal_recurse-inl.h | 16 ++- include/fcl/narrowphase/distance-inl.h | 6 +- .../fcl/narrowphase/distance_request-inl.h | 4 +- include/fcl/narrowphase/distance_result-inl.h | 4 +- src/broadphase/broadphase_SSaP.cpp | 3 +- src/broadphase/broadphase_SaP.cpp | 3 +- src/broadphase/broadphase_bruteforce.cpp | 3 +- .../broadphase_collision_manager.cpp | 3 +- ...roadphase_continuous_collision_manager.cpp | 3 +- .../broadphase_dynamic_AABB_tree.cpp | 3 +- .../broadphase_dynamic_AABB_tree_array.cpp | 3 +- src/broadphase/broadphase_interval_tree.cpp | 3 +- src/broadphase/broadphase_spatialhash.cpp | 3 +- src/broadphase/detail/interval_tree.cpp | 5 +- src/broadphase/detail/interval_tree_node.cpp | 3 +- src/broadphase/detail/morton.cpp | 7 +- src/broadphase/detail/simple_interval.cpp | 3 +- src/broadphase/detail/spatial_hash.cpp | 3 +- src/geometry/bvh/BVH_utility.cpp | 5 +- src/geometry/collision_geometry.cpp | 3 +- src/geometry/octree/octree.cpp | 5 +- src/geometry/shape/box.cpp | 3 +- src/geometry/shape/capsule.cpp | 3 +- src/geometry/shape/cone.cpp | 3 +- src/geometry/shape/convex.cpp | 3 +- src/geometry/shape/cylinder.cpp | 3 +- src/geometry/shape/ellipsoid.cpp | 3 +- src/geometry/shape/halfspace.cpp | 5 +- src/geometry/shape/plane.cpp | 5 +- src/geometry/shape/shape_base.cpp | 3 +- src/geometry/shape/sphere.cpp | 3 +- src/geometry/shape/triangle_p.cpp | 3 +- src/geometry/shape/utility.cpp | 91 ++++++------- src/math/bv/AABB.cpp | 3 +- src/math/bv/OBB.cpp | 13 +- src/math/bv/OBBRSS.cpp | 5 +- src/math/bv/RSS.cpp | 15 ++- src/math/bv/kDOP.cpp | 17 +-- src/math/bv/kIOS.cpp | 3 +- src/math/bv/utility.cpp | 63 ++++----- src/math/detail/polysolver.cpp | 3 +- src/math/detail/project.cpp | 3 +- src/math/geometry.cpp | 35 ++--- src/math/motion/interp_motion.cpp | 3 +- src/math/motion/motion_base.cpp | 3 +- src/math/motion/screw_motion.cpp | 3 +- src/math/motion/spline_motion.cpp | 3 +- src/math/motion/taylor_model/interval.cpp | 7 +- .../motion/taylor_model/interval_matrix.cpp | 5 +- .../motion/taylor_model/interval_vector.cpp | 7 +- .../motion/taylor_model/taylor_matrix.cpp | 17 +-- src/math/motion/taylor_model/taylor_model.cpp | 15 ++- .../motion/taylor_model/taylor_vector.cpp | 11 +- .../motion/taylor_model/time_interval.cpp | 3 +- src/math/motion/translation_motion.cpp | 3 +- .../motion/triangle_motion_bound_visitor.cpp | 3 +- src/math/rng.cpp | 3 +- src/math/sampler/sampler_base.cpp | 3 +- src/math/sampler/sampler_se2.cpp | 3 +- src/math/sampler/sampler_se2_disk.cpp | 3 +- src/math/sampler/sampler_se3_euler.cpp | 3 +- src/math/sampler/sampler_se3_euler_ball.cpp | 3 +- src/math/sampler/sampler_se3_quat.cpp | 3 +- src/math/sampler/sampler_se3_quat_ball.cpp | 3 +- src/math/variance3.cpp | 3 +- src/narrowphase/collision.cpp | 5 +- src/narrowphase/collision_object.cpp | 3 +- src/narrowphase/collision_request.cpp | 3 +- src/narrowphase/collision_result.cpp | 3 +- src/narrowphase/contact.cpp | 3 +- src/narrowphase/contact_point.cpp | 7 +- src/narrowphase/continuous_collision.cpp | 9 +- .../continuous_collision_object.cpp | 3 +- .../continuous_collision_request.cpp | 3 +- .../continuous_collision_result.cpp | 3 +- src/narrowphase/cost_source.cpp | 3 +- .../detail/convexity_based_algorithm/epa.cpp | 3 +- .../detail/convexity_based_algorithm/gjk.cpp | 3 +- .../convexity_based_algorithm/gjk_libccd.cpp | 25 ++-- .../minkowski_diff.cpp | 3 +- src/narrowphase/detail/gjk_solver_indep.cpp | 3 +- src/narrowphase/detail/gjk_solver_libccd.cpp | 3 +- .../primitive_shape_algorithm/box_box.cpp | 11 +- .../capsule_capsule.cpp | 7 +- .../primitive_shape_algorithm/halfspace.cpp | 27 ++-- .../primitive_shape_algorithm/intersect.cpp | 3 +- .../primitive_shape_algorithm/plane.cpp | 23 ++-- .../primitive_shape_algorithm/sphere_box.cpp | 5 +- .../sphere_capsule.cpp | 7 +- .../sphere_cylinder.cpp | 5 +- .../sphere_sphere.cpp | 5 +- .../sphere_triangle.cpp | 13 +- .../triangle_distance.cpp | 3 +- .../collision_traversal_node_base.cpp | 3 +- .../mesh_collision_traversal_node.cpp | 17 +-- ...sh_continuous_collision_traversal_node.cpp | 3 +- .../detail/traversal/collision_node.cpp | 11 +- .../conservative_advancement_stack_data.cpp | 3 +- .../distance/distance_traversal_node_base.cpp | 3 +- ...onservative_advancement_traversal_node.cpp | 9 +- .../distance/mesh_distance_traversal_node.cpp | 13 +- .../detail/traversal/traversal_node_base.cpp | 3 +- .../detail/traversal/traversal_recurse.cpp | 15 ++- src/narrowphase/distance.cpp | 5 +- src/narrowphase/distance_request.cpp | 3 +- src/narrowphase/distance_result.cpp | 3 +- 220 files changed, 1053 insertions(+), 689 deletions(-) diff --git a/include/fcl/CMakeLists.txt b/include/fcl/CMakeLists.txt index 1dab9dc54..f9065caa0 100644 --- a/include/fcl/CMakeLists.txt +++ b/include/fcl/CMakeLists.txt @@ -35,20 +35,12 @@ set(GENERATED_FILE_MARKER "GENERATED FILE DO NOT EDIT") configure_file(config.h.in config.h @ONLY) -set(FCL_EXPORT_MACRO_NAME "FCL_API") +set(FCL_EXPORT_MACRO_NAME "FCL_EXPORT") set(EXPORT_INSTANTIATION_TEMPLATE_MACROS " -#ifdef ${PROJECT_NAME}_EXPORTS - /* We are building this library */ -# ifdef _MSC_VER -# define FCL_EXTERN_TEMPLATE_API -# define FCL_INSTANTIATION_DEF_API ${FCL_EXPORT_MACRO_NAME} -# else -# define FCL_EXTERN_TEMPLATE_API ${FCL_EXPORT_MACRO_NAME} -# define FCL_INSTANTIATION_DEF_API -# endif +#if defined(${PROJECT_NAME}_EXPORTS) && defined(_MSC_VER) +# define FCL_EXPORT_EXPL_INST_DECL #else - /* We are using this library */ -# define FCL_EXTERN_TEMPLATE_API ${FCL_EXPORT_MACRO_NAME} +# define FCL_EXPORT_EXPL_INST_DECL ${FCL_EXPORT_MACRO_NAME} #endif ") generate_export_header(${PROJECT_NAME} diff --git a/include/fcl/broadphase/broadphase_SSaP-inl.h b/include/fcl/broadphase/broadphase_SSaP-inl.h index 4a3b67568..361c3d403 100644 --- a/include/fcl/broadphase/broadphase_SSaP-inl.h +++ b/include/fcl/broadphase/broadphase_SSaP-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_BROADPHASE_SSAP_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SSaPCollisionManager; +class FCL_EXPORT_EXPL_INST_DECL SSaPCollisionManager; +#endif /** @brief Functor sorting objects according to the AABB lower x bound */ template diff --git a/include/fcl/broadphase/broadphase_SaP-inl.h b/include/fcl/broadphase/broadphase_SaP-inl.h index 3bfaa9588..2fd882d58 100644 --- a/include/fcl/broadphase/broadphase_SaP-inl.h +++ b/include/fcl/broadphase/broadphase_SaP-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_BROADPHASE_SAP_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SaPCollisionManager; +class FCL_EXPORT_EXPL_INST_DECL SaPCollisionManager; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_bruteforce-inl.h b/include/fcl/broadphase/broadphase_bruteforce-inl.h index 89aa74816..00574a3e9 100644 --- a/include/fcl/broadphase/broadphase_bruteforce-inl.h +++ b/include/fcl/broadphase/broadphase_bruteforce-inl.h @@ -45,8 +45,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_BROADPHASE_BRUTEFORCE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API NaiveCollisionManager; +class FCL_EXPORT_EXPL_INST_DECL NaiveCollisionManager; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_collision_manager-inl.h b/include/fcl/broadphase/broadphase_collision_manager-inl.h index 7ab9b1fde..a6e1be5b5 100644 --- a/include/fcl/broadphase/broadphase_collision_manager-inl.h +++ b/include/fcl/broadphase/broadphase_collision_manager-inl.h @@ -45,8 +45,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_BROADPHASE_COLLISION_MANAGER_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API BroadPhaseCollisionManager; +class FCL_EXPORT_EXPL_INST_DECL BroadPhaseCollisionManager; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_continuous_collision_manager-inl.h b/include/fcl/broadphase/broadphase_continuous_collision_manager-inl.h index e5033b793..03eb21b07 100644 --- a/include/fcl/broadphase/broadphase_continuous_collision_manager-inl.h +++ b/include/fcl/broadphase/broadphase_continuous_collision_manager-inl.h @@ -45,8 +45,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_BROADPHASE_CONTINUOUS_COLLISION_MANAGER_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API BroadPhaseContinuousCollisionManager; +class FCL_EXPORT_EXPL_INST_DECL BroadPhaseContinuousCollisionManager; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h index 7f3defa89..5d3ff7d6c 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -49,8 +49,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_BROADPHASE_DYNAMIC_AABB_TREE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API DynamicAABBTreeCollisionManager; +class FCL_EXPORT_EXPL_INST_DECL DynamicAABBTreeCollisionManager; +#endif namespace detail { diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index 845579835..b0fcb9ad9 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -48,8 +48,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_BROADPHASE_DYNAMIC_AABB_TREE_ARRAY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API DynamicAABBTreeCollisionManager_Array; +class FCL_EXPORT_EXPL_INST_DECL DynamicAABBTreeCollisionManager_Array; +#endif namespace detail { diff --git a/include/fcl/broadphase/broadphase_interval_tree-inl.h b/include/fcl/broadphase/broadphase_interval_tree-inl.h index f959a1982..b51e3174b 100644 --- a/include/fcl/broadphase/broadphase_interval_tree-inl.h +++ b/include/fcl/broadphase/broadphase_interval_tree-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_BROADPHASE_INTERVAL_TREE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API IntervalTreeCollisionManager; +class FCL_EXPORT_EXPL_INST_DECL IntervalTreeCollisionManager; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/broadphase_spatialhash-inl.h b/include/fcl/broadphase/broadphase_spatialhash-inl.h index 3bb86f02a..a2008ba11 100644 --- a/include/fcl/broadphase/broadphase_spatialhash-inl.h +++ b/include/fcl/broadphase/broadphase_spatialhash-inl.h @@ -44,11 +44,13 @@ namespace fcl { //============================================================================== +#ifndef FCL_BROADPHASE_SPATIALHASH_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SpatialHashingCollisionManager< +class FCL_EXPORT_EXPL_INST_DECL SpatialHashingCollisionManager< double, detail::SimpleHashTable< AABB, CollisionObject*, detail::SpatialHash>>; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/detail/interval_tree-inl.h b/include/fcl/broadphase/detail/interval_tree-inl.h index 2529883ff..380308c98 100644 --- a/include/fcl/broadphase/detail/interval_tree-inl.h +++ b/include/fcl/broadphase/detail/interval_tree-inl.h @@ -46,8 +46,10 @@ namespace fcl { namespace detail { //============================================================================== +#ifndef FCL_BROADPHASE_DETAIL_INTERVAL_TREE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API IntervalTree; +class FCL_EXPORT_EXPL_INST_DECL IntervalTree; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/detail/interval_tree.h b/include/fcl/broadphase/detail/interval_tree.h index 714dac7a8..d7b3c9047 100644 --- a/include/fcl/broadphase/detail/interval_tree.h +++ b/include/fcl/broadphase/detail/interval_tree.h @@ -64,8 +64,10 @@ struct it_recursion_node using it_recursion_nodef = it_recursion_node; using it_recursion_noded = it_recursion_node; +#ifndef FCL_BROADPHASE_DETAIL_INTERVAL_TREE_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API it_recursion_node; +struct FCL_EXPORT_EXPL_INST_DECL it_recursion_node; +#endif /// @brief Interval tree template diff --git a/include/fcl/broadphase/detail/interval_tree_node-inl.h b/include/fcl/broadphase/detail/interval_tree_node-inl.h index f06de88ff..069419a31 100644 --- a/include/fcl/broadphase/detail/interval_tree_node-inl.h +++ b/include/fcl/broadphase/detail/interval_tree_node-inl.h @@ -46,8 +46,10 @@ namespace fcl { namespace detail { //============================================================================== +#ifndef FCL_BROADPHASE_DETAIL_INTERVAL_TREE_NODE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API IntervalTreeNode; +class FCL_EXPORT_EXPL_INST_DECL IntervalTreeNode; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/detail/morton-inl.h b/include/fcl/broadphase/detail/morton-inl.h index 73521786e..ce7bf5889 100644 --- a/include/fcl/broadphase/detail/morton-inl.h +++ b/include/fcl/broadphase/detail/morton-inl.h @@ -46,17 +46,19 @@ namespace fcl { namespace detail { //============================================================================== +#ifndef FCL_BROADPHASE_DETAIL_MORTON_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL uint32 quantize(double x, uint32 n); //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API morton_functor; +struct FCL_EXPORT_EXPL_INST_DECL morton_functor; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API morton_functor; +struct FCL_EXPORT_EXPL_INST_DECL morton_functor; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/detail/morton.h b/include/fcl/broadphase/detail/morton.h index ebbc1c718..7a840e497 100644 --- a/include/fcl/broadphase/detail/morton.h +++ b/include/fcl/broadphase/detail/morton.h @@ -55,11 +55,11 @@ template uint32 quantize(S x, uint32 n); /// @brief compute 30 bit morton code -FCL_API +FCL_EXPORT uint32 morton_code(uint32 x, uint32 y, uint32 z); /// @brief compute 60 bit morton code -FCL_API +FCL_EXPORT uint64 morton_code60(uint32 x, uint32 y, uint32 z); /// @brief Functor to compute the morton code for a given AABB diff --git a/include/fcl/broadphase/detail/simple_interval-inl.h b/include/fcl/broadphase/detail/simple_interval-inl.h index c136bb345..284d92372 100644 --- a/include/fcl/broadphase/detail/simple_interval-inl.h +++ b/include/fcl/broadphase/detail/simple_interval-inl.h @@ -44,8 +44,10 @@ namespace fcl { namespace detail { //============================================================================== +#ifndef FCL_BROADPHASE_DETAIL_SIMPLE_INTERVAL_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API SimpleInterval; +struct FCL_EXPORT_EXPL_INST_DECL SimpleInterval; +#endif //============================================================================== template diff --git a/include/fcl/broadphase/detail/spatial_hash-inl.h b/include/fcl/broadphase/detail/spatial_hash-inl.h index b97354f4b..e5089f5ad 100644 --- a/include/fcl/broadphase/detail/spatial_hash-inl.h +++ b/include/fcl/broadphase/detail/spatial_hash-inl.h @@ -44,8 +44,10 @@ namespace fcl { namespace detail { //============================================================================== +#ifndef FCL_BROADPHASE_DETAIL_SPATIAL_HASH_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API SpatialHash; +struct FCL_EXPORT_EXPL_INST_DECL SpatialHash; +#endif //============================================================================== template diff --git a/include/fcl/common/detail/profiler.h b/include/fcl/common/detail/profiler.h index 7d91706b3..76e3675b6 100644 --- a/include/fcl/common/detail/profiler.h +++ b/include/fcl/common/detail/profiler.h @@ -59,7 +59,7 @@ namespace detail { /// external profiling tools in that it allows the user to count /// time spent in various bits of code (sub-function granularity) /// or count how many times certain pieces of code are executed. -class FCL_API Profiler +class FCL_EXPORT Profiler { public: // non-copyable @@ -68,13 +68,13 @@ class FCL_API Profiler /// @brief This instance will call Profiler::begin() when constructed and /// Profiler::end() when it goes out of scope. - class FCL_API ScopedBlock; + class FCL_EXPORT ScopedBlock; /// @brief This instance will call Profiler::start() when constructed and /// Profiler::stop() when it goes out of scope. /// If the profiler was already started, this block's constructor and /// destructor take no action - class FCL_API ScopedStart; + class FCL_EXPORT ScopedStart; /// @brief Return an instance of the class static Profiler& Instance(void); @@ -211,7 +211,7 @@ class FCL_API Profiler /// @brief This instance will call Profiler::begin() when constructed and /// Profiler::end() when it goes out of scope. -class FCL_API Profiler::ScopedBlock +class FCL_EXPORT Profiler::ScopedBlock { public: /// @brief Start counting time for the block named \e name of the profiler @@ -230,7 +230,7 @@ class FCL_API Profiler::ScopedBlock /// Profiler::stop() when it goes out of scope. /// If the profiler was already started, this block's constructor and /// destructor take no action -class FCL_API Profiler::ScopedStart +class FCL_EXPORT Profiler::ScopedStart { public: diff --git a/include/fcl/common/exception.h b/include/fcl/common/exception.h index d55d352b8..00da2ccfa 100644 --- a/include/fcl/common/exception.h +++ b/include/fcl/common/exception.h @@ -46,7 +46,7 @@ namespace fcl { -class FCL_API Exception : public std::runtime_error +class FCL_EXPORT Exception : public std::runtime_error { public: diff --git a/include/fcl/common/time.h b/include/fcl/common/time.h index 9d4af9b9b..7b040c518 100644 --- a/include/fcl/common/time.h +++ b/include/fcl/common/time.h @@ -55,15 +55,15 @@ using point = std::chrono::system_clock::time_point; using duration = std::chrono::system_clock::duration; /// @brief Get the current time point -FCL_API +FCL_EXPORT point now(void); /// @brief Return the time duration representing a given number of seconds -FCL_API +FCL_EXPORT duration seconds(double sec); /// @brief Return the number of seconds that a time duration represents -FCL_API +FCL_EXPORT double seconds(const duration &d); } // namespace time diff --git a/include/fcl/geometry/bvh/BVH_utility-inl.h b/include/fcl/geometry/bvh/BVH_utility-inl.h index f6e1ea5e8..51d81fa33 100644 --- a/include/fcl/geometry/bvh/BVH_utility-inl.h +++ b/include/fcl/geometry/bvh/BVH_utility-inl.h @@ -46,16 +46,18 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_BHV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void BVHExpand( BVHModel>& model, const Variance3* ucs, double r); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void BVHExpand( BVHModel>& model, const Variance3* ucs, double r); +#endif //============================================================================== template diff --git a/include/fcl/geometry/bvh/BV_node_base.h b/include/fcl/geometry/bvh/BV_node_base.h index 63d9e4be9..a64e67977 100644 --- a/include/fcl/geometry/bvh/BV_node_base.h +++ b/include/fcl/geometry/bvh/BV_node_base.h @@ -47,7 +47,7 @@ namespace fcl { /// @brief BVNodeBase encodes the tree structure for BVH -struct FCL_API BVNodeBase +struct FCL_EXPORT BVNodeBase { /// @brief An index for first child node or primitive /// If the value is positive, it is the index of the first child bv node diff --git a/include/fcl/geometry/bvh/detail/BVH_front.h b/include/fcl/geometry/bvh/detail/BVH_front.h index 9e3a1ddb9..2415f91a9 100644 --- a/include/fcl/geometry/bvh/detail/BVH_front.h +++ b/include/fcl/geometry/bvh/detail/BVH_front.h @@ -52,7 +52,7 @@ namespace detail /// the traversal terminates while performing a query during a given time /// instance. The front list reflects the subset of a BVTT that is traversed for /// that particular proximity query. -struct FCL_API BVHFrontNode +struct FCL_EXPORT BVHFrontNode { /// @brief The nodes to start in the future, i.e. the wave front of the /// traversal tree. @@ -69,7 +69,7 @@ struct FCL_API BVHFrontNode using BVHFrontList = std::list; /// @brief Add new front node into the front list -FCL_API +FCL_EXPORT void updateFrontList(BVHFrontList* front_list, int b1, int b2); } // namespace detail diff --git a/include/fcl/geometry/collision_geometry-inl.h b/include/fcl/geometry/collision_geometry-inl.h index 3a0fded2f..a41a41952 100644 --- a/include/fcl/geometry/collision_geometry-inl.h +++ b/include/fcl/geometry/collision_geometry-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_COLLISION_GEOMETRY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API CollisionGeometry; +class FCL_EXPORT_EXPL_INST_DECL CollisionGeometry; +#endif //============================================================================== template diff --git a/include/fcl/geometry/octree/octree-inl.h b/include/fcl/geometry/octree/octree-inl.h index 7a0434f9b..8d1a9b1e7 100644 --- a/include/fcl/geometry/octree/octree-inl.h +++ b/include/fcl/geometry/octree/octree-inl.h @@ -50,13 +50,15 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_OCTREE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API OcTree; +class FCL_EXPORT_EXPL_INST_DECL OcTree; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv); +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/box-inl.h b/include/fcl/geometry/shape/box-inl.h index b991852cb..d14ef592a 100644 --- a/include/fcl/geometry/shape/box-inl.h +++ b/include/fcl/geometry/shape/box-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_BOX_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Box; +class FCL_EXPORT_EXPL_INST_DECL Box; +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/capsule-inl.h b/include/fcl/geometry/shape/capsule-inl.h index a2e0b8a3a..1721b1f22 100644 --- a/include/fcl/geometry/shape/capsule-inl.h +++ b/include/fcl/geometry/shape/capsule-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_CAPSULE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Capsule; +class FCL_EXPORT_EXPL_INST_DECL Capsule; +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/cone-inl.h b/include/fcl/geometry/shape/cone-inl.h index 08766603f..467e62bb4 100644 --- a/include/fcl/geometry/shape/cone-inl.h +++ b/include/fcl/geometry/shape/cone-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_CONE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Cone; +class FCL_EXPORT_EXPL_INST_DECL Cone; +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/convex-inl.h b/include/fcl/geometry/shape/convex-inl.h index e2f01eaab..3fb8d9477 100644 --- a/include/fcl/geometry/shape/convex-inl.h +++ b/include/fcl/geometry/shape/convex-inl.h @@ -49,8 +49,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_CONVEX_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Convex; +class FCL_EXPORT_EXPL_INST_DECL Convex; +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/cylinder-inl.h b/include/fcl/geometry/shape/cylinder-inl.h index d019f9501..c401ababb 100644 --- a/include/fcl/geometry/shape/cylinder-inl.h +++ b/include/fcl/geometry/shape/cylinder-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_CYLINDER_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Cylinder; +class FCL_EXPORT_EXPL_INST_DECL Cylinder; +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/ellipsoid-inl.h b/include/fcl/geometry/shape/ellipsoid-inl.h index fda3f3f3a..9cb3f31e5 100644 --- a/include/fcl/geometry/shape/ellipsoid-inl.h +++ b/include/fcl/geometry/shape/ellipsoid-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_ELLIPSOID_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Ellipsoid; +class FCL_EXPORT_EXPL_INST_DECL Ellipsoid; +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/halfspace-inl.h b/include/fcl/geometry/shape/halfspace-inl.h index 08e66de9b..4ea7b2a6a 100644 --- a/include/fcl/geometry/shape/halfspace-inl.h +++ b/include/fcl/geometry/shape/halfspace-inl.h @@ -44,13 +44,15 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_HALFSPACE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Halfspace; +class FCL_EXPORT_EXPL_INST_DECL Halfspace; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL Halfspace transform(const Halfspace& a, const Transform3& tf); +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/plane-inl.h b/include/fcl/geometry/shape/plane-inl.h index ef2bb94b1..ed1e9fa32 100644 --- a/include/fcl/geometry/shape/plane-inl.h +++ b/include/fcl/geometry/shape/plane-inl.h @@ -44,13 +44,15 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_PLANE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Plane; +class FCL_EXPORT_EXPL_INST_DECL Plane; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL Plane transform(const Plane& a, const Transform3& tf); +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/shape_base-inl.h b/include/fcl/geometry/shape/shape_base-inl.h index a5defdad4..07f1342b0 100644 --- a/include/fcl/geometry/shape/shape_base-inl.h +++ b/include/fcl/geometry/shape/shape_base-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_BASE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ShapeBase; +class FCL_EXPORT_EXPL_INST_DECL ShapeBase; +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/sphere-inl.h b/include/fcl/geometry/shape/sphere-inl.h index 75487c55f..7d8715710 100644 --- a/include/fcl/geometry/shape/sphere-inl.h +++ b/include/fcl/geometry/shape/sphere-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_SPHERE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Sphere; +class FCL_EXPORT_EXPL_INST_DECL Sphere; +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/triangle_p-inl.h b/include/fcl/geometry/shape/triangle_p-inl.h index 7f9af5d34..e1fab6787 100644 --- a/include/fcl/geometry/shape/triangle_p-inl.h +++ b/include/fcl/geometry/shape/triangle_p-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_TRIANGLE_P_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API TriangleP; +class FCL_EXPORT_EXPL_INST_DECL TriangleP; +#endif //============================================================================== template diff --git a/include/fcl/geometry/shape/utility-inl.h b/include/fcl/geometry/shape/utility-inl.h index 6dd23242f..8b1c10959 100644 --- a/include/fcl/geometry/shape/utility-inl.h +++ b/include/fcl/geometry/shape/utility-inl.h @@ -57,84 +57,86 @@ namespace fcl { //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const AABB& bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const OBB& bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const OBBRSS& bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const kIOS& bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const RSS& bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const AABB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const OBB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const OBBRSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const kIOS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const RSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); +#endif //============================================================================== namespace detail { @@ -334,120 +336,122 @@ struct ComputeBVImpl, TriangleP> }; //============================================================================== +#ifndef FCL_GEOMETRY_SHAPE_UTILITY_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Box>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Box>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Box>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Box>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Capsule>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Capsule>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Capsule>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Capsule>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Cone>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Cone>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Cone>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Cone>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Cylinder>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Cylinder>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Cylinder>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Cylinder>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Ellipsoid>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Ellipsoid>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Ellipsoid>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Ellipsoid>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Halfspace>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Plane>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Plane>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Plane>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Plane>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Plane>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Plane>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Plane>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Plane>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Plane>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Sphere>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Sphere>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, Sphere>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, Sphere>; //============================================================================== extern template -struct FCL_EXTERN_TEMPLATE_API ComputeBVImpl, TriangleP>; +struct FCL_EXPORT_EXPL_INST_DECL ComputeBVImpl, TriangleP>; +#endif //============================================================================== template diff --git a/include/fcl/math/bv/AABB-inl.h b/include/fcl/math/bv/AABB-inl.h index 5b59b15f2..f023acef5 100644 --- a/include/fcl/math/bv/AABB-inl.h +++ b/include/fcl/math/bv/AABB-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_BV_AABB_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API AABB; +class FCL_EXPORT_EXPL_INST_DECL AABB; +#endif //============================================================================== template diff --git a/include/fcl/math/bv/OBB-inl.h b/include/fcl/math/bv/OBB-inl.h index e3bb97f81..0d92ddd3d 100644 --- a/include/fcl/math/bv/OBB-inl.h +++ b/include/fcl/math/bv/OBB-inl.h @@ -46,27 +46,28 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_BV_OBB_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API OBB; +class FCL_EXPORT_EXPL_INST_DECL OBB; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void computeVertices(const OBB& b, Vector3 vertices[8]); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL OBB merge_largedist(const OBB& b1, const OBB& b2); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL OBB merge_smalldist(const OBB& b1, const OBB& b2); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool obbDisjoint( const Matrix3& B, const Vector3& T, @@ -75,11 +76,12 @@ bool obbDisjoint( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool obbDisjoint( const Transform3& tf, const Vector3& a, const Vector3& b); +#endif //============================================================================== template diff --git a/include/fcl/math/bv/OBBRSS-inl.h b/include/fcl/math/bv/OBBRSS-inl.h index fbfe042b0..40d1b56d1 100644 --- a/include/fcl/math/bv/OBBRSS-inl.h +++ b/include/fcl/math/bv/OBBRSS-inl.h @@ -44,13 +44,15 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_BV_OBBRSS_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API OBBRSS; +class FCL_EXPORT_EXPL_INST_DECL OBBRSS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL OBBRSS translate(const OBBRSS& bv, const Vector3& t); +#endif //============================================================================== template diff --git a/include/fcl/math/bv/RSS-inl.h b/include/fcl/math/bv/RSS-inl.h index 92dcf1697..0debf38f3 100644 --- a/include/fcl/math/bv/RSS-inl.h +++ b/include/fcl/math/bv/RSS-inl.h @@ -44,17 +44,18 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_BV_RSS_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API RSS; +class FCL_EXPORT_EXPL_INST_DECL RSS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void clipToRange(double& val, double a, double b); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void segCoords( double& t, double& u, @@ -66,7 +67,7 @@ void segCoords( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool inVoronoi( double a, double b, @@ -78,7 +79,7 @@ bool inVoronoi( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double rectDistance( const Matrix3& Rab, const Vector3& Tab, @@ -89,7 +90,7 @@ double rectDistance( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double rectDistance( const Transform3& tfab, const double a[2], @@ -99,8 +100,9 @@ double rectDistance( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL RSS translate(const RSS& bv, const Vector3& t); +#endif //============================================================================== template diff --git a/include/fcl/math/bv/kDOP-inl.h b/include/fcl/math/bv/kDOP-inl.h index 1a89b4b86..23327cbe1 100644 --- a/include/fcl/math/bv/kDOP-inl.h +++ b/include/fcl/math/bv/kDOP-inl.h @@ -46,41 +46,43 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_BV_KDOP_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API KDOP; +class FCL_EXPORT_EXPL_INST_DECL KDOP; //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API KDOP; +class FCL_EXPORT_EXPL_INST_DECL KDOP; //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API KDOP; +class FCL_EXPORT_EXPL_INST_DECL KDOP; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void minmax(double a, double b, double& minv, double& maxv); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void minmax(double p, double& minv, double& maxv); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void getDistances(const Vector3& p, double* d); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void getDistances(const Vector3& p, double* d); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void getDistances(const Vector3& p, double* d); +#endif //============================================================================== template diff --git a/include/fcl/math/bv/kIOS-inl.h b/include/fcl/math/bv/kIOS-inl.h index 285048d18..68eae5d45 100644 --- a/include/fcl/math/bv/kIOS-inl.h +++ b/include/fcl/math/bv/kIOS-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_BV_KIOS_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API kIOS; +class FCL_EXPORT_EXPL_INST_DECL kIOS; +#endif //============================================================================== template diff --git a/include/fcl/math/bv/utility-inl.h b/include/fcl/math/bv/utility-inl.h index f933e8460..63bf8a9a1 100644 --- a/include/fcl/math/bv/utility-inl.h +++ b/include/fcl/math/bv/utility-inl.h @@ -64,41 +64,51 @@ namespace OBB_fit_functions { template void fit1(const Vector3* ps, OBB& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit1(const Vector3d* ps, OBB& bv); +#endif //============================================================================== template void fit2(const Vector3* ps, OBB& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit2(const Vector3d* ps, OBB& bv); +#endif //============================================================================== template void fit3(const Vector3* ps, OBB& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit3(const Vector3d* ps, OBB& bv); +#endif //============================================================================== template void fit6(const Vector3* ps, OBB& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit6(const Vector3d* ps, OBB& bv); +#endif //============================================================================== template void fitn(const Vector3* ps, int n, OBB& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fitn(const Vector3d* ps, int n, OBB& bv); +#endif //============================================================================== template @@ -191,41 +201,51 @@ namespace RSS_fit_functions { template void fit1(const Vector3* ps, RSS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit1(const Vector3d* ps, RSS& bv); +#endif //============================================================================== template void fit2(const Vector3* ps, RSS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit2(const Vector3d* ps, RSS& bv); +#endif //============================================================================== template void fit3(const Vector3* ps, RSS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit3(const Vector3d* ps, RSS& bv); +#endif //============================================================================== template void fit6(const Vector3* ps, RSS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit6(const Vector3d* ps, RSS& bv); +#endif //============================================================================== template void fitn(const Vector3* ps, int n, RSS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fitn(const Vector3d* ps, int n, RSS& bv); +#endif //============================================================================== template @@ -320,33 +340,41 @@ namespace kIOS_fit_functions { template void fit1(const Vector3* ps, kIOS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit1(const Vector3d* ps, kIOS& bv); +#endif //============================================================================== template void fit2(const Vector3* ps, kIOS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit2(const Vector3d* ps, kIOS& bv); +#endif //============================================================================== template void fit3(const Vector3* ps, kIOS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit3(const Vector3d* ps, kIOS& bv); +#endif //============================================================================== template void fitn(const Vector3* ps, int n, kIOS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fitn(const Vector3d* ps, int n, kIOS& bv); +#endif //============================================================================== template @@ -520,33 +548,41 @@ namespace OBBRSS_fit_functions { template void fit1(const Vector3* ps, OBBRSS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit1(const Vector3d* ps, OBBRSS& bv); +#endif //============================================================================== template void fit2(const Vector3* ps, OBBRSS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit2(const Vector3d* ps, OBBRSS& bv); +#endif //============================================================================== template void fit3(const Vector3* ps, OBBRSS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fit3(const Vector3d* ps, OBBRSS& bv); +#endif //============================================================================== template void fitn(const Vector3* ps, int n, OBBRSS& bv); +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void fitn(const Vector3d* ps, int n, OBBRSS& bv); +#endif //============================================================================== template @@ -598,8 +634,10 @@ struct Fitter> static void fit(const Vector3* ps, int n, OBB& bv); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API Fitter>; +struct FCL_EXPORT_EXPL_INST_DECL Fitter>; +#endif //============================================================================== template @@ -608,8 +646,10 @@ struct Fitter> static void fit(const Vector3* ps, int n, RSS& bv); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API Fitter>; +struct FCL_EXPORT_EXPL_INST_DECL Fitter>; +#endif //============================================================================== template @@ -618,8 +658,10 @@ struct Fitter> static void fit(const Vector3* ps, int n, kIOS& bv); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API Fitter>; +struct FCL_EXPORT_EXPL_INST_DECL Fitter>; +#endif //============================================================================== template @@ -628,8 +670,10 @@ struct Fitter> static void fit(const Vector3* ps, int n, OBBRSS& bv); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API Fitter>; +struct FCL_EXPORT_EXPL_INST_DECL Fitter>; +#endif //============================================================================== template @@ -764,8 +808,10 @@ class ConvertBVImpl, AABB> static void run(const AABB& bv1, const Transform3& tf1, AABB& bv2); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, AABB>; +class FCL_EXPORT_EXPL_INST_DECL ConvertBVImpl, AABB>; +#endif //============================================================================== template @@ -783,8 +829,10 @@ class ConvertBVImpl, OBB> static void run(const AABB& bv1, const Transform3& tf1, OBB& bv2); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; +class FCL_EXPORT_EXPL_INST_DECL ConvertBVImpl, OBB>; +#endif //============================================================================== template @@ -794,8 +842,10 @@ class ConvertBVImpl, OBB> static void run(const OBB& bv1, const Transform3& tf1, OBB& bv2); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; +class FCL_EXPORT_EXPL_INST_DECL ConvertBVImpl, OBB>; +#endif //============================================================================== template @@ -805,8 +855,10 @@ class ConvertBVImpl, OBB> static void run(const OBBRSS& bv1, const Transform3& tf1, OBB& bv2); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; +class FCL_EXPORT_EXPL_INST_DECL ConvertBVImpl, OBB>; +#endif //============================================================================== template @@ -816,8 +868,10 @@ class ConvertBVImpl, OBB> static void run(const RSS& bv1, const Transform3& tf1, OBB& bv2); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, OBB>; +class FCL_EXPORT_EXPL_INST_DECL ConvertBVImpl, OBB>; +#endif //============================================================================== template @@ -827,8 +881,10 @@ class ConvertBVImpl, RSS> static void run(const OBB& bv1, const Transform3& tf1, RSS& bv2); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; +class FCL_EXPORT_EXPL_INST_DECL ConvertBVImpl, RSS>; +#endif //============================================================================== template @@ -838,8 +894,10 @@ class ConvertBVImpl, RSS> static void run(const RSS& bv1, const Transform3& tf1, RSS& bv2); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; +class FCL_EXPORT_EXPL_INST_DECL ConvertBVImpl, RSS>; +#endif //============================================================================== template @@ -849,8 +907,10 @@ class ConvertBVImpl, RSS> static void run(const OBBRSS& bv1, const Transform3& tf1, RSS& bv2); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; +class FCL_EXPORT_EXPL_INST_DECL ConvertBVImpl, RSS>; +#endif //============================================================================== template @@ -860,8 +920,10 @@ class ConvertBVImpl, RSS> static void run(const AABB& bv1, const Transform3& tf1, RSS& bv2); }; +#ifndef FCL_MATH_BV_UTILITY_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ConvertBVImpl, RSS>; +class FCL_EXPORT_EXPL_INST_DECL ConvertBVImpl, RSS>; +#endif //============================================================================== template diff --git a/include/fcl/math/detail/polysolver-inl.h b/include/fcl/math/detail/polysolver-inl.h index 405fea6e3..985bb2fef 100644 --- a/include/fcl/math/detail/polysolver-inl.h +++ b/include/fcl/math/detail/polysolver-inl.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H -#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H +#ifndef FCL_MATH_DETAIL_POLYSOLVER_INL_H +#define FCL_MATH_DETAIL_POLYSOLVER_INL_H #include "fcl/math/detail/polysolver.h" @@ -49,8 +49,10 @@ namespace fcl namespace detail { //============================================================================== +#ifndef FCL_MATH_DETAIL_POLYSOLVER_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API PolySolver; +class FCL_EXPORT_EXPL_INST_DECL PolySolver; +#endif //============================================================================== template diff --git a/include/fcl/math/detail/polysolver.h b/include/fcl/math/detail/polysolver.h index bcad09f86..8d86b5575 100644 --- a/include/fcl/math/detail/polysolver.h +++ b/include/fcl/math/detail/polysolver.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef FCL_NARROWPHASE_DETAIL_POLYSOLVER_H -#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_H +#ifndef FCL_MATH_DETAIL_POLYSOLVER_H +#define FCL_MATH_DETAIL_POLYSOLVER_H #include "fcl/export.h" diff --git a/include/fcl/math/detail/project-inl.h b/include/fcl/math/detail/project-inl.h index 1b29dffc2..0f9ac1650 100644 --- a/include/fcl/math/detail/project-inl.h +++ b/include/fcl/math/detail/project-inl.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_INL_H -#define FCL_NARROWPHASE_DETAIL_PROJECT_INL_H +#ifndef FCL_MATH_DETAIL_PROJECT_INL_H +#define FCL_MATH_DETAIL_PROJECT_INL_H #include "fcl/math/detail/project.h" @@ -47,8 +47,10 @@ namespace detail { //============================================================================== +#ifndef FCL_MATH_DETAIL_PROJECT_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Project; +class FCL_EXPORT_EXPL_INST_DECL Project; +#endif //============================================================================== template diff --git a/include/fcl/math/detail/project.h b/include/fcl/math/detail/project.h index 4b19b0dbe..848a47fc9 100644 --- a/include/fcl/math/detail/project.h +++ b/include/fcl/math/detail/project.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef FCL_NARROWPHASE_DETAIL_PROJECT_H -#define FCL_NARROWPHASE_DETAIL_PROJECT_H +#ifndef FCL_MATH_DETAIL_PROJECT_H +#define FCL_MATH_DETAIL_PROJECT_H #include "fcl/common/types.h" #include "fcl/math/geometry.h" diff --git a/include/fcl/math/detail/seed.h b/include/fcl/math/detail/seed.h index df76dd88e..3ec4eeb68 100644 --- a/include/fcl/math/detail/seed.h +++ b/include/fcl/math/detail/seed.h @@ -46,7 +46,7 @@ namespace fcl namespace detail { -class FCL_API Seed +class FCL_EXPORT Seed { public: diff --git a/include/fcl/math/geometry-inl.h b/include/fcl/math/geometry-inl.h index f93e49776..9b605333d 100644 --- a/include/fcl/math/geometry-inl.h +++ b/include/fcl/math/geometry-inl.h @@ -35,8 +35,8 @@ /** @author Jia Pan */ -#ifndef FCL_BV_DETAIL_UTILITY_INL_H -#define FCL_BV_DETAIL_UTILITY_INL_H +#ifndef FCL_MATH_GEOMETRY_INL_H +#define FCL_MATH_GEOMETRY_INL_H #include "fcl/math/geometry.h" #include "fcl/math/constants.h" @@ -44,46 +44,47 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_GEOMETRY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void normalize(Vector3d& v, bool* signal); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void hat(Matrix3d& mat, const Vector3d& vec); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void eigen(const Matrix3d& m, Vector3d& dout, Matrix3d& vout); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void eigen_old(const Matrix3d& m, Vector3d& dout, Matrix3d& vout); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void axisFromEigen( const Matrix3d& eigenV, const Vector3d& eigenS, Matrix3d& axis); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void axisFromEigen(const Matrix3d& eigenV, const Vector3d& eigenS, Transform3d& tf); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL Matrix3d generateCoordinateSystem(const Vector3d& x_axis); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void getRadiusAndOriginAndRectangleSize( const Vector3d* const ps, const Vector3d* const ps2, @@ -97,7 +98,7 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void getRadiusAndOriginAndRectangleSize( const Vector3d* const ps, const Vector3d* const ps2, @@ -110,7 +111,7 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void circumCircleComputation( const Vector3d& a, const Vector3d& b, @@ -120,7 +121,7 @@ void circumCircleComputation( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double maximumDistance( const Vector3d* const ps, const Vector3d* const ps2, @@ -131,7 +132,7 @@ double maximumDistance( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void getExtentAndCenter( const Vector3d* const ps, const Vector3d* const ps2, @@ -144,13 +145,14 @@ void getExtentAndCenter( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void getCovariance( const Vector3d* const ps, const Vector3d* const ps2, Triangle* ts, unsigned int* indices, int n, Matrix3d& M); +#endif //============================================================================== namespace detail { @@ -371,8 +373,9 @@ void getExtentAndCenter_mesh( } //============================================================================== +#ifndef FCL_MATH_GEOMETRY_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double maximumDistance_mesh( const Vector3d* const ps, const Vector3d* const ps2, @@ -383,7 +386,7 @@ double maximumDistance_mesh( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double maximumDistance_pointcloud( const Vector3d* const ps, const Vector3d* const ps2, @@ -393,7 +396,7 @@ double maximumDistance_pointcloud( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void getExtentAndCenter_pointcloud( const Vector3d* const ps, const Vector3d* const ps2, @@ -405,7 +408,7 @@ void getExtentAndCenter_pointcloud( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void getExtentAndCenter_mesh( const Vector3d* const ps, const Vector3d* const ps2, @@ -415,6 +418,7 @@ void getExtentAndCenter_mesh( const Matrix3d& axis, Vector3d& center, Vector3d& extent); +#endif //============================================================================== } // namespace detail diff --git a/include/fcl/math/motion/interp_motion-inl.h b/include/fcl/math/motion/interp_motion-inl.h index e25b99fe6..db77ae926 100644 --- a/include/fcl/math/motion/interp_motion-inl.h +++ b/include/fcl/math/motion/interp_motion-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_INTERP_MOTION_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API InterpMotion; +class FCL_EXPORT_EXPL_INST_DECL InterpMotion; +#endif //============================================================================== template diff --git a/include/fcl/math/motion/motion_base-inl.h b/include/fcl/math/motion/motion_base-inl.h index 88bde2bd9..d021f6782 100644 --- a/include/fcl/math/motion/motion_base-inl.h +++ b/include/fcl/math/motion/motion_base-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_BASE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API MotionBase; +class FCL_EXPORT_EXPL_INST_DECL MotionBase; +#endif //============================================================================== template diff --git a/include/fcl/math/motion/screw_motion-inl.h b/include/fcl/math/motion/screw_motion-inl.h index 13650400c..417629919 100644 --- a/include/fcl/math/motion/screw_motion-inl.h +++ b/include/fcl/math/motion/screw_motion-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_SCREW_MOTION_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ScrewMotion; +class FCL_EXPORT_EXPL_INST_DECL ScrewMotion; +#endif //============================================================================== template diff --git a/include/fcl/math/motion/spline_motion-inl.h b/include/fcl/math/motion/spline_motion-inl.h index c9b5bf457..b76783e12 100644 --- a/include/fcl/math/motion/spline_motion-inl.h +++ b/include/fcl/math/motion/spline_motion-inl.h @@ -46,8 +46,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_SPLINE_MOTION_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SplineMotion; +class FCL_EXPORT_EXPL_INST_DECL SplineMotion; +#endif //============================================================================== template diff --git a/include/fcl/math/motion/taylor_model/interval-inl.h b/include/fcl/math/motion/taylor_model/interval-inl.h index d1350cf0e..7ec251ee5 100644 --- a/include/fcl/math/motion/taylor_model/interval-inl.h +++ b/include/fcl/math/motion/taylor_model/interval-inl.h @@ -45,18 +45,20 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_TAYLOR_MODEL_INTERVAL_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API Interval; +struct FCL_EXPORT_EXPL_INST_DECL Interval; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL Interval bound(const Interval& i, double v); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL Interval bound(const Interval& i, const Interval& other); +#endif //============================================================================== template diff --git a/include/fcl/math/motion/taylor_model/interval_matrix-inl.h b/include/fcl/math/motion/taylor_model/interval_matrix-inl.h index 0ce69295c..3f11a8a65 100644 --- a/include/fcl/math/motion/taylor_model/interval_matrix-inl.h +++ b/include/fcl/math/motion/taylor_model/interval_matrix-inl.h @@ -44,13 +44,15 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_TAYLOR_MODEL_INTERVAL_MATRIX_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API IMatrix3; +struct FCL_EXPORT_EXPL_INST_DECL IMatrix3; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL IMatrix3 rotationConstrain(const IMatrix3& m); +#endif //============================================================================== template diff --git a/include/fcl/math/motion/taylor_model/interval_vector-inl.h b/include/fcl/math/motion/taylor_model/interval_vector-inl.h index ffcc77e15..d05979cf8 100644 --- a/include/fcl/math/motion/taylor_model/interval_vector-inl.h +++ b/include/fcl/math/motion/taylor_model/interval_vector-inl.h @@ -44,18 +44,20 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_TAYLOR_MODEL_INTERVAL_VECTOR_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API IVector3; +struct FCL_EXPORT_EXPL_INST_DECL IVector3; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL IVector3 bound(const IVector3& i, const Vector3& v); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL IVector3 bound(const IVector3& i, const IVector3& v); +#endif //============================================================================== template diff --git a/include/fcl/math/motion/taylor_model/taylor_matrix-inl.h b/include/fcl/math/motion/taylor_model/taylor_matrix-inl.h index 592b00c4d..0dbb30b6b 100644 --- a/include/fcl/math/motion/taylor_model/taylor_matrix-inl.h +++ b/include/fcl/math/motion/taylor_model/taylor_matrix-inl.h @@ -44,43 +44,45 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_TAYLOR_MODEL_TAYLOR_MATRIX_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API TMatrix3; +class FCL_EXPORT_EXPL_INST_DECL TMatrix3; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TMatrix3 rotationConstrain(const TMatrix3& m); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TMatrix3 operator * (const Matrix3& m, const TaylorModel& a); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TMatrix3 operator * (const TaylorModel& a, const Matrix3& m); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TMatrix3 operator * (double d, const TMatrix3& m); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TMatrix3 operator + (const Matrix3& m1, const TMatrix3& m2); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TMatrix3 operator - (const Matrix3& m1, const TMatrix3& m2); +#endif //============================================================================== template diff --git a/include/fcl/math/motion/taylor_model/taylor_model-inl.h b/include/fcl/math/motion/taylor_model/taylor_model-inl.h index 8d37c4d52..6c33f307d 100644 --- a/include/fcl/math/motion/taylor_model/taylor_model-inl.h +++ b/include/fcl/math/motion/taylor_model/taylor_model-inl.h @@ -47,38 +47,40 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_TAYLOR_MODEL_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API TaylorModel; +class FCL_EXPORT_EXPL_INST_DECL TaylorModel; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TaylorModel operator * (double d, const TaylorModel& a); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TaylorModel operator + (double d, const TaylorModel& a); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TaylorModel operator - (double d, const TaylorModel& a); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void generateTaylorModelForCosFunc(TaylorModel& tm, double w, double q0); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void generateTaylorModelForSinFunc(TaylorModel& tm, double w, double q0); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void generateTaylorModelForLinearFunc(TaylorModel& tm, double p, double v); +#endif //============================================================================== template diff --git a/include/fcl/math/motion/taylor_model/taylor_vector-inl.h b/include/fcl/math/motion/taylor_model/taylor_vector-inl.h index 1bcda4e6b..3d3788656 100644 --- a/include/fcl/math/motion/taylor_model/taylor_vector-inl.h +++ b/include/fcl/math/motion/taylor_model/taylor_vector-inl.h @@ -44,28 +44,30 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_TAYLOR_MODEL_TAYLOR_VECTOR_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API TVector3; +class FCL_EXPORT_EXPL_INST_DECL TVector3; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void generateTVector3ForLinearFunc(TVector3& v, const Vector3& position, const Vector3& velocity); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TVector3 operator * (const Vector3& v, const TaylorModel& a); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TVector3 operator + (const Vector3& v1, const TVector3& v2); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL TVector3 operator - (const Vector3& v1, const TVector3& v2); +#endif //============================================================================== template diff --git a/include/fcl/math/motion/taylor_model/time_interval-inl.h b/include/fcl/math/motion/taylor_model/time_interval-inl.h index 2aead4d33..71ec1284c 100644 --- a/include/fcl/math/motion/taylor_model/time_interval-inl.h +++ b/include/fcl/math/motion/taylor_model/time_interval-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_TAYLOR_MODEL_TIME_INTERVAL_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API TimeInterval; +struct FCL_EXPORT_EXPL_INST_DECL TimeInterval; +#endif //============================================================================== template diff --git a/include/fcl/math/motion/translation_motion-inl.h b/include/fcl/math/motion/translation_motion-inl.h index ceb350094..402598fe5 100644 --- a/include/fcl/math/motion/translation_motion-inl.h +++ b/include/fcl/math/motion/translation_motion-inl.h @@ -46,8 +46,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_TRANSLATION_MOTION_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API TranslationMotion; +class FCL_EXPORT_EXPL_INST_DECL TranslationMotion; +#endif //============================================================================== template diff --git a/include/fcl/math/motion/triangle_motion_bound_visitor-inl.h b/include/fcl/math/motion/triangle_motion_bound_visitor-inl.h index 29212d1a8..e4c5fc474 100644 --- a/include/fcl/math/motion/triangle_motion_bound_visitor-inl.h +++ b/include/fcl/math/motion/triangle_motion_bound_visitor-inl.h @@ -50,8 +50,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_MOTION_TRIANGLE_MOTION_BOUND_VISITOR_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API TriangleMotionBoundVisitor; +class FCL_EXPORT_EXPL_INST_DECL TriangleMotionBoundVisitor; +#endif //============================================================================== template diff --git a/include/fcl/math/rng-inl.h b/include/fcl/math/rng-inl.h index 8b32b226e..4732f34a3 100644 --- a/include/fcl/math/rng-inl.h +++ b/include/fcl/math/rng-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_RNG_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API RNG; +class FCL_EXPORT_EXPL_INST_DECL RNG; +#endif //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_base.h b/include/fcl/math/sampler/sampler_base.h index 3150971ce..dbbc83a8e 100644 --- a/include/fcl/math/sampler/sampler_base.h +++ b/include/fcl/math/sampler/sampler_base.h @@ -50,8 +50,10 @@ class SamplerBase mutable RNG rng; }; +#ifndef FCL_MATH_SAMPLER_BASE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SamplerBase; +class FCL_EXPORT_EXPL_INST_DECL SamplerBase; +#endif } // namespace fcl diff --git a/include/fcl/math/sampler/sampler_se2-inl.h b/include/fcl/math/sampler/sampler_se2-inl.h index 78428d4ca..6c089ac0d 100644 --- a/include/fcl/math/sampler/sampler_se2-inl.h +++ b/include/fcl/math/sampler/sampler_se2-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_SAMPLER_SE2_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SamplerSE2; +class FCL_EXPORT_EXPL_INST_DECL SamplerSE2; +#endif //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se2_disk-inl.h b/include/fcl/math/sampler/sampler_se2_disk-inl.h index 6575e828b..8dc389244 100644 --- a/include/fcl/math/sampler/sampler_se2_disk-inl.h +++ b/include/fcl/math/sampler/sampler_se2_disk-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_SAMPLER_SE2_DISK_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SamplerSE2_disk; +class FCL_EXPORT_EXPL_INST_DECL SamplerSE2_disk; +#endif //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se3_euler-inl.h b/include/fcl/math/sampler/sampler_se3_euler-inl.h index 4ece176f6..2ddc5425c 100644 --- a/include/fcl/math/sampler/sampler_se3_euler-inl.h +++ b/include/fcl/math/sampler/sampler_se3_euler-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_SAMPLER_SE3_EULER_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SamplerSE3Euler; +class FCL_EXPORT_EXPL_INST_DECL SamplerSE3Euler; +#endif //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se3_euler_ball-inl.h b/include/fcl/math/sampler/sampler_se3_euler_ball-inl.h index 69e9169a4..2554e6b77 100644 --- a/include/fcl/math/sampler/sampler_se3_euler_ball-inl.h +++ b/include/fcl/math/sampler/sampler_se3_euler_ball-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_SAMPLER_SE3_EULER_BALL_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SamplerSE3Euler_ball; +class FCL_EXPORT_EXPL_INST_DECL SamplerSE3Euler_ball; +#endif //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se3_quat-inl.h b/include/fcl/math/sampler/sampler_se3_quat-inl.h index dba3e0be0..160494401 100644 --- a/include/fcl/math/sampler/sampler_se3_quat-inl.h +++ b/include/fcl/math/sampler/sampler_se3_quat-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_SAMPLER_SE3_QUAT_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SamplerSE3Quat; +class FCL_EXPORT_EXPL_INST_DECL SamplerSE3Quat; +#endif //============================================================================== template diff --git a/include/fcl/math/sampler/sampler_se3_quat_ball-inl.h b/include/fcl/math/sampler/sampler_se3_quat_ball-inl.h index 2fe34bccb..31aa0c252 100644 --- a/include/fcl/math/sampler/sampler_se3_quat_ball-inl.h +++ b/include/fcl/math/sampler/sampler_se3_quat_ball-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_SAMPLER_SE3_QUAT_BALL_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API SamplerSE3Quat_ball; +class FCL_EXPORT_EXPL_INST_DECL SamplerSE3Quat_ball; +#endif //============================================================================== template diff --git a/include/fcl/math/triangle.h b/include/fcl/math/triangle.h index a639a606d..f7ce26a5f 100644 --- a/include/fcl/math/triangle.h +++ b/include/fcl/math/triangle.h @@ -45,7 +45,7 @@ namespace fcl { /// @brief Triangle with 3 indices for points -class FCL_API Triangle +class FCL_EXPORT Triangle { /// @brief indices for each vertex of triangle std::size_t vids[3]; diff --git a/include/fcl/math/variance3-inl.h b/include/fcl/math/variance3-inl.h index a0e61cd46..5a568af13 100644 --- a/include/fcl/math/variance3-inl.h +++ b/include/fcl/math/variance3-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_MATH_VARIANCE3_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Variance3; +class FCL_EXPORT_EXPL_INST_DECL Variance3; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/collision-inl.h b/include/fcl/narrowphase/collision-inl.h index cd3de19f9..2eaea8f5a 100644 --- a/include/fcl/narrowphase/collision-inl.h +++ b/include/fcl/narrowphase/collision-inl.h @@ -48,8 +48,9 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_COLLISION_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL std::size_t collide( const CollisionObject* o1, const CollisionObject* o2, @@ -58,7 +59,7 @@ std::size_t collide( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL std::size_t collide( const CollisionGeometry* o1, const Transform3& tf1, @@ -66,6 +67,7 @@ std::size_t collide( const Transform3& tf2, const CollisionRequest& request, CollisionResult& result); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/collision_object-inl.h b/include/fcl/narrowphase/collision_object-inl.h index 54d7b9502..82fde42b8 100644 --- a/include/fcl/narrowphase/collision_object-inl.h +++ b/include/fcl/narrowphase/collision_object-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_COLLISION_OBJECT_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API CollisionObject; +class FCL_EXPORT_EXPL_INST_DECL CollisionObject; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/collision_request-inl.h b/include/fcl/narrowphase/collision_request-inl.h index 3aa7a21b2..501c96012 100644 --- a/include/fcl/narrowphase/collision_request-inl.h +++ b/include/fcl/narrowphase/collision_request-inl.h @@ -46,8 +46,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_COLLISION_REQUEST_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API CollisionRequest; +struct FCL_EXPORT_EXPL_INST_DECL CollisionRequest; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/collision_result-inl.h b/include/fcl/narrowphase/collision_result-inl.h index 9b52aa2f9..a89ff06f4 100644 --- a/include/fcl/narrowphase/collision_result-inl.h +++ b/include/fcl/narrowphase/collision_result-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_COLLISION_RESULT_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API CollisionResult; +struct FCL_EXPORT_EXPL_INST_DECL CollisionResult; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/contact-inl.h b/include/fcl/narrowphase/contact-inl.h index 4bf729089..039b1aa82 100644 --- a/include/fcl/narrowphase/contact-inl.h +++ b/include/fcl/narrowphase/contact-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_CONTACT_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API Contact; +struct FCL_EXPORT_EXPL_INST_DECL Contact; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/contact_point-inl.h b/include/fcl/narrowphase/contact_point-inl.h index fba2432ae..98c91c2d9 100644 --- a/include/fcl/narrowphase/contact_point-inl.h +++ b/include/fcl/narrowphase/contact_point-inl.h @@ -44,19 +44,21 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_CONTACT_POINT_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API ContactPoint; +struct FCL_EXPORT_EXPL_INST_DECL ContactPoint; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool comparePenDepth( const ContactPoint& _cp1, const ContactPoint& _cp2); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void flipNormal(std::vector>& contacts); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/continuous_collision-inl.h b/include/fcl/narrowphase/continuous_collision-inl.h index bf124a7ea..1784ecbc4 100644 --- a/include/fcl/narrowphase/continuous_collision-inl.h +++ b/include/fcl/narrowphase/continuous_collision-inl.h @@ -55,8 +55,9 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_CONTINUOUS_COLLISION_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double continuousCollide( const CollisionGeometry* o1, const MotionBase* motion1, @@ -67,7 +68,7 @@ double continuousCollide( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double continuousCollide( const CollisionGeometry* o1, const Transform3& tf1_beg, @@ -80,7 +81,7 @@ double continuousCollide( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double continuousCollide( const CollisionObject* o1, const Transform3& tf1_end, @@ -91,12 +92,13 @@ double continuousCollide( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double collide( const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, const ContinuousCollisionRequest& request, ContinuousCollisionResult& result); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/continuous_collision_object-inl.h b/include/fcl/narrowphase/continuous_collision_object-inl.h index 0b39838b3..94660488e 100644 --- a/include/fcl/narrowphase/continuous_collision_object-inl.h +++ b/include/fcl/narrowphase/continuous_collision_object-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_CONTINUOUS_COLLISION_OBJECT_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API ContinuousCollisionObject; +class FCL_EXPORT_EXPL_INST_DECL ContinuousCollisionObject; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/continuous_collision_request-inl.h b/include/fcl/narrowphase/continuous_collision_request-inl.h index 5096710c8..a2545e700 100644 --- a/include/fcl/narrowphase/continuous_collision_request-inl.h +++ b/include/fcl/narrowphase/continuous_collision_request-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_CONTINUOUS_COLLISION_REQUEST_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API ContinuousCollisionRequest; +struct FCL_EXPORT_EXPL_INST_DECL ContinuousCollisionRequest; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/continuous_collision_result-inl.h b/include/fcl/narrowphase/continuous_collision_result-inl.h index ff0416002..56bd2fda1 100644 --- a/include/fcl/narrowphase/continuous_collision_result-inl.h +++ b/include/fcl/narrowphase/continuous_collision_result-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_CONTINUOUS_COLLISION_RESULT_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API ContinuousCollisionResult; +struct FCL_EXPORT_EXPL_INST_DECL ContinuousCollisionResult; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/cost_source-inl.h b/include/fcl/narrowphase/cost_source-inl.h index 31b90bacf..c83a47f8b 100644 --- a/include/fcl/narrowphase/cost_source-inl.h +++ b/include/fcl/narrowphase/cost_source-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_COST_SOURCE_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API CostSource; +struct FCL_EXPORT_EXPL_INST_DECL CostSource; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa-inl.h index d1b2b648c..c25cad794 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa-inl.h @@ -47,8 +47,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_CONVEXITY_BASED_ALGORITHM_EPA_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API EPA; +struct FCL_EXPORT_EXPL_INST_DECL EPA; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk-inl.h index 768ea39e3..5333a27f3 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk-inl.h @@ -47,8 +47,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_CONVEXITY_BASED_ALGORITHM_GJK_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API GJK; +struct FCL_EXPORT_EXPL_INST_DECL GJK; +#endif //============================================================================== template 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 70119ce8b..435062bfe 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 @@ -55,42 +55,43 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_CONVEXITY_BASED_ALGORITHM_GJK_LIBCCD_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API GJKInitializer>; +class FCL_EXPORT_EXPL_INST_DECL GJKInitializer>; //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API GJKInitializer>; +class FCL_EXPORT_EXPL_INST_DECL GJKInitializer>; //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API GJKInitializer>; +class FCL_EXPORT_EXPL_INST_DECL GJKInitializer>; //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API GJKInitializer>; +class FCL_EXPORT_EXPL_INST_DECL GJKInitializer>; //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API GJKInitializer>; +class FCL_EXPORT_EXPL_INST_DECL GJKInitializer>; //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API GJKInitializer>; +class FCL_EXPORT_EXPL_INST_DECL GJKInitializer>; //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API GJKInitializer>; +class FCL_EXPORT_EXPL_INST_DECL GJKInitializer>; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void* triCreateGJKObject( const Vector3d& P1, const Vector3d& P2, const Vector3d& P3); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void* triCreateGJKObject( const Vector3d& P1, const Vector3d& P2, @@ -99,7 +100,7 @@ void* triCreateGJKObject( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool GJKCollide( void* obj1, ccd_support_fn supp1, @@ -115,7 +116,7 @@ bool GJKCollide( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool GJKDistance( void* obj1, ccd_support_fn supp1, @@ -128,7 +129,7 @@ bool GJKDistance( Vector3d* p2); extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool GJKSignedDistance( void* obj1, ccd_support_fn supp1, @@ -139,6 +140,7 @@ bool GJKSignedDistance( double* dist, Vector3d* p1, Vector3d* p2); +#endif struct ccd_obj_t { diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h index d72707ccf..3bcecd36a 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h @@ -169,10 +169,10 @@ class GJKInitializer> }; /// @brief initialize GJK Triangle -FCL_API +FCL_EXPORT GJKSupportFunction triGetSupportFunction(); -FCL_API +FCL_EXPORT GJKCenterFunction triGetCenterFunction(); template @@ -181,7 +181,7 @@ void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vecto template void* triCreateGJKObject(const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf); -FCL_API +FCL_EXPORT void triDeleteGJKObject(void* o); /// @brief GJK collision algorithm diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h index abceb5704..69bde039c 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h @@ -58,8 +58,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_CONVEXITY_BASED_ALGORITHM_MINKOWSKI_DIFF_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API MinkowskiDiff; +struct FCL_EXPORT_EXPL_INST_DECL MinkowskiDiff; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/failed_at_this_configuration.h b/include/fcl/narrowphase/detail/failed_at_this_configuration.h index 1856fa2c3..b36bb5952 100644 --- a/include/fcl/narrowphase/detail/failed_at_this_configuration.h +++ b/include/fcl/narrowphase/detail/failed_at_this_configuration.h @@ -61,7 +61,7 @@ namespace detail { FCL_THROW_UNEXPECTED_CONFIGURATION_EXCEPTION defined below. Code that exercises functionality that throws this type of exception should catch it and transform it to a `std::logic_error` by invoking ThrowDetailedConfiguration(). */ -class FCL_API FailedAtThisConfiguration final +class FCL_EXPORT FailedAtThisConfiguration final : public std::exception { public: FailedAtThisConfiguration(const std::string& message) @@ -82,7 +82,7 @@ class FCL_API FailedAtThisConfiguration final @param func The name of the invoking function. @param file The name of the file associated with the exception. @param line The line number where the exception is thrown. */ -FCL_API void ThrowFailedAtThisConfiguration( +FCL_EXPORT void ThrowFailedAtThisConfiguration( const std::string& message, const char* func, const char* file, int line); /** Helper class for propagating a low-level exception upwards but with diff --git a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h index 46a52be82..e357e7132 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_indep-inl.h @@ -66,8 +66,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_GJK_SOLVER_INDEP_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API GJKSolver_indep; +struct FCL_EXPORT_EXPL_INST_DECL GJKSolver_indep; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h index 0ff1e902f..e096b9bd9 100755 --- a/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h +++ b/include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h @@ -63,8 +63,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_GJK_SOLVER_LIBCCD_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API GJKSolver_libccd; +struct FCL_EXPORT_EXPL_INST_DECL GJKSolver_libccd; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h index a9ea0cee1..b7d7b4ae5 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h @@ -49,25 +49,26 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_BOX_BOX_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void lineClosestApproach(const Vector3& pa, const Vector3& ua, const Vector3& pb, const Vector3& ub, double* alpha, double* beta); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL int intersectRectQuad2(double h[2], double p[8], double ret[16]); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void cullPoints2(int n, double p[], int m, int i0, int iret[]); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL int boxBox2( const Vector3& side1, const Transform3& tf1, @@ -81,10 +82,11 @@ int boxBox2( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool boxBoxIntersect(const Box& s1, const Transform3& tf1, const Box& s2, const Transform3& tf2, std::vector>* contacts_); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h index dea64afc9..14f1e2dc0 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h @@ -47,13 +47,14 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_CAPSULE_CAPSULE_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double clamp(double n, double min, double max); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double closestPtSegmentSegment(const Vector3d& p_FP1, const Vector3d& p_FQ1, const Vector3d& p_FP2, @@ -63,11 +64,12 @@ double closestPtSegmentSegment(const Vector3d& p_FP1, //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool capsuleCapsuleDistance( const Capsule& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, double* dist, Vector3d* p1_res, Vector3d* p2_res); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h index 51e4dcdd3..1bdd6b758 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h @@ -47,8 +47,9 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_HALFSPACE_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereHalfspaceIntersect( const Sphere& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -56,7 +57,7 @@ bool sphereHalfspaceIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool ellipsoidHalfspaceIntersect( const Ellipsoid& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -64,14 +65,14 @@ bool ellipsoidHalfspaceIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool boxHalfspaceIntersect( const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool boxHalfspaceIntersect( const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -79,7 +80,7 @@ bool boxHalfspaceIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool capsuleHalfspaceIntersect( const Capsule& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -87,7 +88,7 @@ bool capsuleHalfspaceIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool cylinderHalfspaceIntersect( const Cylinder& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -95,7 +96,7 @@ bool cylinderHalfspaceIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool coneHalfspaceIntersect( const Cone& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -103,7 +104,7 @@ bool coneHalfspaceIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool convexHalfspaceIntersect( const Convex& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -111,7 +112,7 @@ bool convexHalfspaceIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool convexHalfspaceIntersect( const Convex& convex_C, const Transform3& X_FC, const Halfspace& half_space_H, const Transform3& X_FH, @@ -119,7 +120,7 @@ bool convexHalfspaceIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool halfspaceTriangleIntersect( const Halfspace& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, @@ -127,7 +128,7 @@ bool halfspaceTriangleIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool planeHalfspaceIntersect( const Plane& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -138,7 +139,7 @@ bool planeHalfspaceIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool halfspacePlaneIntersect( const Halfspace& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, @@ -148,7 +149,7 @@ bool halfspacePlaneIntersect( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool halfspaceIntersect( const Halfspace& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -156,6 +157,7 @@ bool halfspaceIntersect( Halfspace& s, double& penetration_depth, int& ret); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h index e9e3795b8..76cb169a9 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/halfspace.h @@ -59,11 +59,11 @@ template S halfspaceIntersectTolerance(); template <> -FCL_API +FCL_EXPORT float halfspaceIntersectTolerance(); template <> -FCL_API +FCL_EXPORT double halfspaceIntersectTolerance(); template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h index 6b6d35f39..3ddead420 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h @@ -47,79 +47,81 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_PLANE_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool spherePlaneIntersect(const Sphere& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool boxPlaneIntersect(const Box& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool conePlaneIntersect(const Cone& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, Vector3* contact_points, double* penetration_depth, Vector3* normal); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool planeTriangleIntersect(const Plane& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, Vector3* contact_points, double* penetration_depth, Vector3* normal); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool planeIntersect(const Plane& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane.h index 073d50978..601456731 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/plane.h @@ -58,11 +58,11 @@ template S planeIntersectTolerance(); template <> -FCL_API +FCL_EXPORT double planeIntersectTolerance(); template <> -FCL_API +FCL_EXPORT float planeIntersectTolerance(); template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h index a46809335..29cfce91d 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h @@ -42,8 +42,9 @@ namespace fcl { namespace detail { +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_BOX_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, const Box& box, const Transform3& X_FB, std::vector>* contacts); @@ -51,11 +52,12 @@ bool sphereBoxIntersect(const Sphere& sphere, const Transform3& //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, const Box& box, const Transform3& X_FB, double* distance, Vector3* p_FSb, Vector3* p_FBs); +#endif //============================================================================== diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h index 210a55135..49b870a32 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h @@ -47,8 +47,9 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_CAPSULE_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void lineSegmentPointClosestToPoint( const Vector3 &p, const Vector3 &s1, @@ -57,17 +58,18 @@ void lineSegmentPointClosestToPoint( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereCapsuleIntersect(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h index 3ffc91726..915a7e64f 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h @@ -42,8 +42,9 @@ namespace fcl { namespace detail { +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_CYLINDER_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereCylinderIntersect(const Sphere& sphere, const Transform3& X_FS, const Cylinder& cylinder, @@ -53,13 +54,14 @@ bool sphereCylinderIntersect(const Sphere& sphere, //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereCylinderDistance(const Sphere& sphere, const Transform3& X_FS, const Cylinder& cylinder, const Transform3& X_FC, double* distance, Vector3* p_FSc, Vector3* p_FCs); +#endif //============================================================================== diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h index 9a5aa7f1b..2331a1993 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h @@ -47,18 +47,20 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_SPHERE_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereSphereIntersect(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereSphereDistance(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle-inl.h index 2899110d5..e6321accf 100755 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle-inl.h @@ -49,41 +49,43 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_TRIANGLE_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double segmentSqrDistance(const Vector3& from, const Vector3& to,const Vector3& p, Vector3& nearest); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool projectInTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& normal, const Vector3& p); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereTriangleIntersect(const Sphere& s, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, Vector3* contact_points, double* penetration_depth, Vector3* normal_); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, double* dist); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, double* dist, Vector3* p1, Vector3* p2); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h index ec58038b3..29e859789 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h @@ -47,8 +47,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_TRIANGLE_DISTANCE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API TriangleDistance; +class FCL_EXPORT_EXPL_INST_DECL TriangleDistance; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base-inl.h b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base-inl.h index 75bddb675..47037fced 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base-inl.h @@ -49,8 +49,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_TRAVERSAL_NODE_BASE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API CollisionTraversalNodeBase; +class FCL_EXPORT_EXPL_INST_DECL CollisionTraversalNodeBase; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h b/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h index 027c791a9..219b7d85d 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h @@ -47,8 +47,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_INTERSECT_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API Intersect; +class FCL_EXPORT_EXPL_INST_DECL Intersect; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h index 21d7d094e..af9dfa97e 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h @@ -51,12 +51,13 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_MESH_COLLISION_TRAVERSAL_NODE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API MeshCollisionTraversalNodeOBB; +class FCL_EXPORT_EXPL_INST_DECL MeshCollisionTraversalNodeOBB; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool initialize( MeshCollisionTraversalNodeOBB& node, const BVHModel>& model1, @@ -68,11 +69,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API MeshCollisionTraversalNodeRSS; +class FCL_EXPORT_EXPL_INST_DECL MeshCollisionTraversalNodeRSS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool initialize( MeshCollisionTraversalNodeRSS& node, const BVHModel>& model1, @@ -84,11 +85,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API MeshCollisionTraversalNodekIOS; +class FCL_EXPORT_EXPL_INST_DECL MeshCollisionTraversalNodekIOS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool initialize( MeshCollisionTraversalNodekIOS& node, const BVHModel>& model1, @@ -100,11 +101,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API MeshCollisionTraversalNodeOBBRSS; +class FCL_EXPORT_EXPL_INST_DECL MeshCollisionTraversalNodeOBBRSS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool initialize( MeshCollisionTraversalNodeOBBRSS& node, const BVHModel>& model1, @@ -113,6 +114,7 @@ bool initialize( const Transform3& tf2, const CollisionRequest& request, CollisionResult& result); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node-inl.h index f3b263a32..eba0d41e6 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node-inl.h @@ -49,8 +49,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_MESH_CONTINUOUS_COLLISION_TRAVERSAL_NODE_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API BVHContinuousCollisionPair; +struct FCL_EXPORT_EXPL_INST_DECL BVHContinuousCollisionPair; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/collision_node-inl.h b/include/fcl/narrowphase/detail/traversal/collision_node-inl.h index 305931c1b..2cc487d72 100644 --- a/include/fcl/narrowphase/detail/traversal/collision_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision_node-inl.h @@ -48,29 +48,31 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_NODE_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data-inl.h b/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data-inl.h index 26e0be7a5..59561e6f6 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data-inl.h @@ -47,8 +47,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_DISTANCE_CONSERVATIVE_ADVANCEMENT_STACK_DATA_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API ConservativeAdvancementStackData; +struct FCL_EXPORT_EXPL_INST_DECL ConservativeAdvancementStackData; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base-inl.h b/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base-inl.h index 5cd460c0a..6e53347c6 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base-inl.h @@ -49,8 +49,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_DISTANCE_TRAVERSAL_NODE_BASE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API DistanceTraversalNodeBase; +class FCL_EXPORT_EXPL_INST_DECL DistanceTraversalNodeBase; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h index 0c5512672..4ce941a53 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h @@ -50,12 +50,13 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_DISTANCE_MESH_CONSERVATIVE_ADVANCEMENT_TRAVERSAL_NODE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API MeshConservativeAdvancementTraversalNodeRSS; +class FCL_EXPORT_EXPL_INST_DECL MeshConservativeAdvancementTraversalNodeRSS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool initialize( MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel>& model1, @@ -66,11 +67,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API MeshConservativeAdvancementTraversalNodeOBBRSS; +class FCL_EXPORT_EXPL_INST_DECL MeshConservativeAdvancementTraversalNodeOBBRSS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool initialize( MeshConservativeAdvancementTraversalNodeOBBRSS& node, const BVHModel>& model1, @@ -78,6 +79,7 @@ bool initialize( const BVHModel>& model2, const Transform3& tf2, double w); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node-inl.h index 012990257..1226d0c02 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node-inl.h @@ -47,12 +47,13 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_DISTANCE_MESH_DISTANCE_TRAVERSAL_NODE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API MeshDistanceTraversalNodeRSS; +class FCL_EXPORT_EXPL_INST_DECL MeshDistanceTraversalNodeRSS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool initialize( MeshDistanceTraversalNodeRSS& node, const BVHModel>& model1, @@ -64,11 +65,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API MeshDistanceTraversalNodekIOS; +class FCL_EXPORT_EXPL_INST_DECL MeshDistanceTraversalNodekIOS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool initialize( MeshDistanceTraversalNodekIOS& node, const BVHModel>& model1, @@ -80,11 +81,11 @@ bool initialize( //============================================================================== extern template -class FCL_EXTERN_TEMPLATE_API MeshDistanceTraversalNodeOBBRSS; +class FCL_EXPORT_EXPL_INST_DECL MeshDistanceTraversalNodeOBBRSS; //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL bool initialize( MeshDistanceTraversalNodeOBBRSS& node, const BVHModel>& model1, @@ -93,6 +94,7 @@ bool initialize( const Transform3& tf2, const DistanceRequest& request, DistanceResult& result); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/traversal_node_base-inl.h b/include/fcl/narrowphase/detail/traversal/traversal_node_base-inl.h index 928071e24..f812a35ae 100644 --- a/include/fcl/narrowphase/detail/traversal/traversal_node_base-inl.h +++ b/include/fcl/narrowphase/detail/traversal/traversal_node_base-inl.h @@ -49,8 +49,10 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_NODE_BASE_BUILDING extern template -class FCL_EXTERN_TEMPLATE_API TraversalNodeBase; +class FCL_EXPORT_EXPL_INST_DECL TraversalNodeBase; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/detail/traversal/traversal_recurse-inl.h b/include/fcl/narrowphase/detail/traversal/traversal_recurse-inl.h index 7d81b0f7c..1cce40833 100644 --- a/include/fcl/narrowphase/detail/traversal/traversal_recurse-inl.h +++ b/include/fcl/narrowphase/detail/traversal/traversal_recurse-inl.h @@ -51,39 +51,41 @@ namespace detail { //============================================================================== +#ifndef FCL_NARROWPHASE_DETAIL_TRAVERSAL_RECURSE_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize); //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/distance-inl.h b/include/fcl/narrowphase/distance-inl.h index 538b92339..a965efebf 100644 --- a/include/fcl/narrowphase/distance-inl.h +++ b/include/fcl/narrowphase/distance-inl.h @@ -46,8 +46,9 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_DISTANCE_BUILDING extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double distance( const CollisionObject* o1, const CollisionObject* o2, @@ -56,11 +57,12 @@ double distance( //============================================================================== extern template -FCL_EXTERN_TEMPLATE_API +FCL_EXPORT_EXPL_INST_DECL double distance( const CollisionGeometry* o1, const Transform3& tf1, const CollisionGeometry* o2, const Transform3& tf2, const DistanceRequest& request, DistanceResult& result); +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/distance_request-inl.h b/include/fcl/narrowphase/distance_request-inl.h index 18d928cac..0f05e83a9 100644 --- a/include/fcl/narrowphase/distance_request-inl.h +++ b/include/fcl/narrowphase/distance_request-inl.h @@ -46,8 +46,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_DISTANCE_REQUEST_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API DistanceRequest; +struct FCL_EXPORT_EXPL_INST_DECL DistanceRequest; +#endif //============================================================================== template diff --git a/include/fcl/narrowphase/distance_result-inl.h b/include/fcl/narrowphase/distance_result-inl.h index 40a26f789..435e98a60 100644 --- a/include/fcl/narrowphase/distance_result-inl.h +++ b/include/fcl/narrowphase/distance_result-inl.h @@ -44,8 +44,10 @@ namespace fcl { //============================================================================== +#ifndef FCL_NARROWPHASE_DISTANCE_RESULT_BUILDING extern template -struct FCL_EXTERN_TEMPLATE_API DistanceResult; +struct FCL_EXPORT_EXPL_INST_DECL DistanceResult; +#endif //============================================================================== template diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index 81df6ad42..ae1511970 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_SSAP_BUILDING #include "fcl/broadphase/broadphase_SSaP-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SSaPCollisionManager; +class FCL_EXPORT SSaPCollisionManager; } // namespace diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index d1975d16e..98af7c892 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_SAP_BUILDING #include "fcl/broadphase/broadphase_SaP-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SaPCollisionManager; +class FCL_EXPORT SaPCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index c86bed19b..acb369cef 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_BRUTEFORCE_BUILDING #include "fcl/broadphase/broadphase_bruteforce-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API NaiveCollisionManager; +class FCL_EXPORT NaiveCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp index 1afb67e69..b60b339b6 100644 --- a/src/broadphase/broadphase_collision_manager.cpp +++ b/src/broadphase/broadphase_collision_manager.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_COLLISION_MANAGER_BUILDING #include "fcl/broadphase/broadphase_collision_manager-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API BroadPhaseCollisionManager; +class FCL_EXPORT BroadPhaseCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_continuous_collision_manager.cpp b/src/broadphase/broadphase_continuous_collision_manager.cpp index e30121133..ce7f0b538 100644 --- a/src/broadphase/broadphase_continuous_collision_manager.cpp +++ b/src/broadphase/broadphase_continuous_collision_manager.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_CONTINUOUS_COLLISION_MANAGER_BUILDING #include "fcl/broadphase/broadphase_continuous_collision_manager-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API BroadPhaseContinuousCollisionManager; +class FCL_EXPORT BroadPhaseContinuousCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 2d51012c0..70a46cad0 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_DYNAMIC_AABB_TREE_BUILDING #include "fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API DynamicAABBTreeCollisionManager; +class FCL_EXPORT DynamicAABBTreeCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 8b9b065f0..26f9967f9 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_DYNAMIC_AABB_TREE_ARRAY_BUILDING #include "fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API DynamicAABBTreeCollisionManager_Array; +class FCL_EXPORT DynamicAABBTreeCollisionManager_Array; } // namespace fcl diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index 70e527b94..e5222ea82 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_INTERVAL_TREE_BUILDING #include "fcl/broadphase/broadphase_interval_tree-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API IntervalTreeCollisionManager; +class FCL_EXPORT IntervalTreeCollisionManager; } // namespace fcl diff --git a/src/broadphase/broadphase_spatialhash.cpp b/src/broadphase/broadphase_spatialhash.cpp index d1fa84f09..778ad0e7c 100644 --- a/src/broadphase/broadphase_spatialhash.cpp +++ b/src/broadphase/broadphase_spatialhash.cpp @@ -35,13 +35,14 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_SPATIALHASH_BUILDING #include "fcl/broadphase/broadphase_spatialhash-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SpatialHashingCollisionManager< +class FCL_EXPORT SpatialHashingCollisionManager< double, detail::SimpleHashTable< AABB, CollisionObject*, detail::SpatialHash>>; diff --git a/src/broadphase/detail/interval_tree.cpp b/src/broadphase/detail/interval_tree.cpp index 1cc1a5346..ce4928cde 100644 --- a/src/broadphase/detail/interval_tree.cpp +++ b/src/broadphase/detail/interval_tree.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_DETAIL_INTERVAL_TREE_BUILDING #include "fcl/broadphase/detail/interval_tree-inl.h" namespace fcl @@ -44,10 +45,10 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API it_recursion_node; +struct FCL_EXPORT it_recursion_node; template -class FCL_INSTANTIATION_DEF_API IntervalTree; +class FCL_EXPORT IntervalTree; } // namespace detail } // namespace fcl diff --git a/src/broadphase/detail/interval_tree_node.cpp b/src/broadphase/detail/interval_tree_node.cpp index 1fe47a325..d3ceb3e03 100644 --- a/src/broadphase/detail/interval_tree_node.cpp +++ b/src/broadphase/detail/interval_tree_node.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_DETAIL_INTERVAL_TREE_NODE_BUILDING #include "fcl/broadphase/detail/interval_tree_node-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -class FCL_INSTANTIATION_DEF_API IntervalTreeNode; +class FCL_EXPORT IntervalTreeNode; } // namespace detail } // namespace fcl diff --git a/src/broadphase/detail/morton.cpp b/src/broadphase/detail/morton.cpp index 035644ccd..58aef5690 100644 --- a/src/broadphase/detail/morton.cpp +++ b/src/broadphase/detail/morton.cpp @@ -36,6 +36,7 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_DETAIL_MORTON_BUILDING #include "fcl/broadphase/detail/morton-inl.h" namespace fcl @@ -47,7 +48,7 @@ namespace detail //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT uint32 quantize(double x, uint32 n); //============================================================================== @@ -87,11 +88,11 @@ uint64 morton_code60(uint32 x, uint32 y, uint32 z) //============================================================================== template -struct FCL_INSTANTIATION_DEF_API morton_functor; +struct FCL_EXPORT morton_functor; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API morton_functor; +struct FCL_EXPORT morton_functor; } // namespace detail /// @endcond diff --git a/src/broadphase/detail/simple_interval.cpp b/src/broadphase/detail/simple_interval.cpp index 01faeed0f..b0a490027 100644 --- a/src/broadphase/detail/simple_interval.cpp +++ b/src/broadphase/detail/simple_interval.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_DETAIL_SIMPLE_INTERVAL_BUILDING #include "fcl/broadphase/detail/simple_interval-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API SimpleInterval; +struct FCL_EXPORT SimpleInterval; } // namespace detail } // namespace fcl diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp index 11a67b3f4..0ad898166 100644 --- a/src/broadphase/detail/spatial_hash.cpp +++ b/src/broadphase/detail/spatial_hash.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_BROADPHASE_DETAIL_SPATIAL_HASH_BUILDING #include "fcl/broadphase/detail/spatial_hash-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API SpatialHash; +struct FCL_EXPORT SpatialHash; } // namespace detail } // namespace fcl diff --git a/src/geometry/bvh/BVH_utility.cpp b/src/geometry/bvh/BVH_utility.cpp index de6e1f013..33e48f17a 100644 --- a/src/geometry/bvh/BVH_utility.cpp +++ b/src/geometry/bvh/BVH_utility.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_BHV_UTILITY_BUILDING #include "fcl/geometry/bvh/BVH_utility-inl.h" namespace fcl @@ -42,13 +43,13 @@ namespace fcl //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void BVHExpand( BVHModel>& model, const Variance3* ucs, double r); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void BVHExpand( BVHModel>& model, const Variance3* ucs, double r); diff --git a/src/geometry/collision_geometry.cpp b/src/geometry/collision_geometry.cpp index 83d96e47a..98799b173 100644 --- a/src/geometry/collision_geometry.cpp +++ b/src/geometry/collision_geometry.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_COLLISION_GEOMETRY_BUILDING #include "fcl/geometry/collision_geometry-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API CollisionGeometry; +class FCL_EXPORT CollisionGeometry; } // namespace fcl diff --git a/src/geometry/octree/octree.cpp b/src/geometry/octree/octree.cpp index 2e27cbb5e..6d5cd4741 100644 --- a/src/geometry/octree/octree.cpp +++ b/src/geometry/octree/octree.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_OCTREE_BUILDING #include "fcl/geometry/octree/octree-inl.h" #include "fcl/config.h" @@ -46,11 +47,11 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API OcTree; +class FCL_EXPORT OcTree; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv); } // namespace fcl diff --git a/src/geometry/shape/box.cpp b/src/geometry/shape/box.cpp index 1d1f4e17b..2ad3d6ff5 100644 --- a/src/geometry/shape/box.cpp +++ b/src/geometry/shape/box.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_BOX_BUILDING #include "fcl/geometry/shape/box-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API Box; +class FCL_EXPORT Box; } // namespace fcl diff --git a/src/geometry/shape/capsule.cpp b/src/geometry/shape/capsule.cpp index 594fe0a65..eb3cb9a82 100644 --- a/src/geometry/shape/capsule.cpp +++ b/src/geometry/shape/capsule.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_CAPSULE_BUILDING #include "fcl/geometry/shape/capsule-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API Capsule; +class FCL_EXPORT Capsule; } // namespace fcl diff --git a/src/geometry/shape/cone.cpp b/src/geometry/shape/cone.cpp index 0bb75d186..2509d6b9e 100644 --- a/src/geometry/shape/cone.cpp +++ b/src/geometry/shape/cone.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_CONE_BUILDING #include "fcl/geometry/shape/cone-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API Cone; +class FCL_EXPORT Cone; } // namespace fcl diff --git a/src/geometry/shape/convex.cpp b/src/geometry/shape/convex.cpp index 7b4fc5b86..f0343ec88 100644 --- a/src/geometry/shape/convex.cpp +++ b/src/geometry/shape/convex.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_CONVEX_BUILDING #include "fcl/geometry/shape/convex-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API Convex; +class FCL_EXPORT Convex; } // namespace fcl diff --git a/src/geometry/shape/cylinder.cpp b/src/geometry/shape/cylinder.cpp index be61aee4d..d09999037 100644 --- a/src/geometry/shape/cylinder.cpp +++ b/src/geometry/shape/cylinder.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_CYLINDER_BUILDING #include "fcl/geometry/shape/cylinder-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API Cylinder; +class FCL_EXPORT Cylinder; } // namespace fcl diff --git a/src/geometry/shape/ellipsoid.cpp b/src/geometry/shape/ellipsoid.cpp index 9e26db205..b223c41a3 100644 --- a/src/geometry/shape/ellipsoid.cpp +++ b/src/geometry/shape/ellipsoid.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_ELLIPSOID_BUILDING #include "fcl/geometry/shape/ellipsoid-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API Ellipsoid; +class FCL_EXPORT Ellipsoid; } // namespace fcl diff --git a/src/geometry/shape/halfspace.cpp b/src/geometry/shape/halfspace.cpp index e887371c1..7f30d0090 100644 --- a/src/geometry/shape/halfspace.cpp +++ b/src/geometry/shape/halfspace.cpp @@ -35,16 +35,17 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_HALFSPACE_BUILDING #include "fcl/geometry/shape/halfspace-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API Halfspace; +class FCL_EXPORT Halfspace; template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT Halfspace transform(const Halfspace& a, const Transform3& tf); } // namespace fcl diff --git a/src/geometry/shape/plane.cpp b/src/geometry/shape/plane.cpp index fe7b5912c..81fcec589 100644 --- a/src/geometry/shape/plane.cpp +++ b/src/geometry/shape/plane.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_PLANE_BUILDING #include "fcl/geometry/shape/plane-inl.h" namespace fcl @@ -42,11 +43,11 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API Plane; +class FCL_EXPORT Plane; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT Plane transform(const Plane& a, const Transform3& tf); } // namespace fcl diff --git a/src/geometry/shape/shape_base.cpp b/src/geometry/shape/shape_base.cpp index 98d65e554..b2b9bb2cc 100644 --- a/src/geometry/shape/shape_base.cpp +++ b/src/geometry/shape/shape_base.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_BASE_BUILDING #include "fcl/geometry/shape/shape_base-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API ShapeBase; +class FCL_EXPORT ShapeBase; } // namespace fcl diff --git a/src/geometry/shape/sphere.cpp b/src/geometry/shape/sphere.cpp index 7d4a9429f..2f2aff0e4 100644 --- a/src/geometry/shape/sphere.cpp +++ b/src/geometry/shape/sphere.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_SPHERE_BUILDING #include "fcl/geometry/shape/sphere-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API Sphere; +class FCL_EXPORT Sphere; } // namespace fcl diff --git a/src/geometry/shape/triangle_p.cpp b/src/geometry/shape/triangle_p.cpp index a375fb879..2fc1ffe02 100644 --- a/src/geometry/shape/triangle_p.cpp +++ b/src/geometry/shape/triangle_p.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_TRIANGLE_P_BUILDING #include "fcl/geometry/shape/triangle_p-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API TriangleP; +class FCL_EXPORT TriangleP; } // namespace fcl diff --git a/src/geometry/shape/utility.cpp b/src/geometry/shape/utility.cpp index e3737b5df..65fa8f99f 100644 --- a/src/geometry/shape/utility.cpp +++ b/src/geometry/shape/utility.cpp @@ -35,88 +35,89 @@ /** @author Jia Pan */ +#define FCL_GEOMETRY_SHAPE_UTILITY_BUILDING #include "fcl/geometry/shape/utility-inl.h" namespace fcl { //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const AABB& bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const OBB& bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const OBBRSS& bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const kIOS& bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const RSS& bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const KDOP& bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const AABB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const OBB& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const OBBRSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const kIOS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const RSS& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void constructBox(const KDOP& bv, const Transform3& tf_bv, Box& box, Transform3& tf); //============================================================================== @@ -125,119 +126,119 @@ namespace detail { //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Box>; +struct FCL_EXPORT ComputeBVImpl, Box>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Box>; +struct FCL_EXPORT ComputeBVImpl, Box>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Capsule>; +struct FCL_EXPORT ComputeBVImpl, Capsule>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Capsule>; +struct FCL_EXPORT ComputeBVImpl, Capsule>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Cone>; +struct FCL_EXPORT ComputeBVImpl, Cone>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Cone>; +struct FCL_EXPORT ComputeBVImpl, Cone>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Cylinder>; +struct FCL_EXPORT ComputeBVImpl, Cylinder>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Cylinder>; +struct FCL_EXPORT ComputeBVImpl, Cylinder>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Ellipsoid>; +struct FCL_EXPORT ComputeBVImpl, Ellipsoid>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Ellipsoid>; +struct FCL_EXPORT ComputeBVImpl, Ellipsoid>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT ComputeBVImpl, Halfspace>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT ComputeBVImpl, Halfspace>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT ComputeBVImpl, Halfspace>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT ComputeBVImpl, Halfspace>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT ComputeBVImpl, Halfspace>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT ComputeBVImpl, Halfspace>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT ComputeBVImpl, Halfspace>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Halfspace>; +struct FCL_EXPORT ComputeBVImpl, Halfspace>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; +struct FCL_EXPORT ComputeBVImpl, Plane>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; +struct FCL_EXPORT ComputeBVImpl, Plane>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; +struct FCL_EXPORT ComputeBVImpl, Plane>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; +struct FCL_EXPORT ComputeBVImpl, Plane>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; +struct FCL_EXPORT ComputeBVImpl, Plane>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; +struct FCL_EXPORT ComputeBVImpl, Plane>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; +struct FCL_EXPORT ComputeBVImpl, Plane>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Plane>; +struct FCL_EXPORT ComputeBVImpl, Plane>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Sphere>; +struct FCL_EXPORT ComputeBVImpl, Sphere>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, Sphere>; +struct FCL_EXPORT ComputeBVImpl, Sphere>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ComputeBVImpl, TriangleP>; +struct FCL_EXPORT ComputeBVImpl, TriangleP>; //============================================================================== } // namespace detail diff --git a/src/math/bv/AABB.cpp b/src/math/bv/AABB.cpp index 9696c758c..844e0894c 100644 --- a/src/math/bv/AABB.cpp +++ b/src/math/bv/AABB.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_BV_AABB_BUILDING #include "fcl/math/bv/AABB-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API AABB; +class FCL_EXPORT AABB; } // namespace fcl diff --git a/src/math/bv/OBB.cpp b/src/math/bv/OBB.cpp index af100579a..f548fdbf0 100644 --- a/src/math/bv/OBB.cpp +++ b/src/math/bv/OBB.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_MATH_BV_OBB_BUILDING #include "fcl/math/bv/OBB-inl.h" namespace fcl @@ -42,26 +43,26 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API OBB; +class FCL_EXPORT OBB; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void computeVertices(const OBB& b, Vector3 vertices[8]); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT OBB merge_largedist(const OBB& b1, const OBB& b2); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT OBB merge_smalldist(const OBB& b1, const OBB& b2); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool obbDisjoint( const Matrix3& B, const Vector3& T, @@ -70,7 +71,7 @@ bool obbDisjoint( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool obbDisjoint( const Transform3& tf, const Vector3& a, diff --git a/src/math/bv/OBBRSS.cpp b/src/math/bv/OBBRSS.cpp index 84278a2f0..48e30acc8 100644 --- a/src/math/bv/OBBRSS.cpp +++ b/src/math/bv/OBBRSS.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_MATH_BV_OBBRSS_BUILDING #include "fcl/math/bv/OBBRSS-inl.h" namespace fcl @@ -42,11 +43,11 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API OBBRSS; +class FCL_EXPORT OBBRSS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT OBBRSS translate(const OBBRSS& bv, const Vector3& t); } // namespace fcl diff --git a/src/math/bv/RSS.cpp b/src/math/bv/RSS.cpp index b2aff7af2..dec171e98 100644 --- a/src/math/bv/RSS.cpp +++ b/src/math/bv/RSS.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_MATH_BV_RSS_BUILDING #include "fcl/math/bv/RSS-inl.h" namespace fcl @@ -42,16 +43,16 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API RSS; +class FCL_EXPORT RSS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void clipToRange(double& val, double a, double b); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void segCoords( double& t, double& u, @@ -63,7 +64,7 @@ void segCoords( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool inVoronoi( double a, double b, @@ -75,7 +76,7 @@ bool inVoronoi( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double rectDistance( const Matrix3& Rab, const Vector3& Tab, @@ -86,7 +87,7 @@ double rectDistance( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double rectDistance( const Transform3& tfab, const double a[2], @@ -96,7 +97,7 @@ double rectDistance( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT RSS translate(const RSS& bv, const Vector3& t); } // namespace fcl diff --git a/src/math/bv/kDOP.cpp b/src/math/bv/kDOP.cpp index e8901914b..6305d9ac5 100644 --- a/src/math/bv/kDOP.cpp +++ b/src/math/bv/kDOP.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_MATH_BV_KDOP_BUILDING #include "fcl/math/bv/kDOP-inl.h" namespace fcl @@ -42,39 +43,39 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API KDOP; +class FCL_EXPORT KDOP; //============================================================================== template -class FCL_INSTANTIATION_DEF_API KDOP; +class FCL_EXPORT KDOP; //============================================================================== template -class FCL_INSTANTIATION_DEF_API KDOP; +class FCL_EXPORT KDOP; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void minmax(double a, double b, double& minv, double& maxv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void minmax(double p, double& minv, double& maxv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void getDistances(const Vector3& p, double* d); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void getDistances(const Vector3& p, double* d); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void getDistances(const Vector3& p, double* d); } // namespace fcl diff --git a/src/math/bv/kIOS.cpp b/src/math/bv/kIOS.cpp index b43a3e5e4..c038a7526 100644 --- a/src/math/bv/kIOS.cpp +++ b/src/math/bv/kIOS.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_BV_KIOS_BUILDING #include "fcl/math/bv/kIOS-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API kIOS; +class FCL_EXPORT kIOS; } // namespace fcl diff --git a/src/math/bv/utility.cpp b/src/math/bv/utility.cpp index 505236598..cef40b1df 100644 --- a/src/math/bv/utility.cpp +++ b/src/math/bv/utility.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_MATH_BV_UTILITY_BUILDING #include "fcl/math/bv/utility-inl.h" namespace fcl { @@ -46,27 +47,27 @@ namespace OBB_fit_functions { //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit1(const Vector3d* const ps, OBB& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit2(const Vector3d* const ps, OBB& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit3(const Vector3d* const ps, OBB& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit6(const Vector3d* const ps, OBB& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fitn(const Vector3d* const ps, int n, OBB& bv); //============================================================================== @@ -79,27 +80,27 @@ namespace RSS_fit_functions { //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit1(const Vector3d* const ps, RSS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit2(const Vector3d* const ps, RSS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit3(const Vector3d* const ps, RSS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit6(const Vector3d* const ps, RSS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fitn(const Vector3d* const ps, int n, RSS& bv); //============================================================================== @@ -112,22 +113,22 @@ namespace kIOS_fit_functions { //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit1(const Vector3d* const ps, kIOS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit2(const Vector3d* const ps, kIOS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit3(const Vector3d* const ps, kIOS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fitn(const Vector3d* const ps, int n, kIOS& bv); //============================================================================== @@ -140,22 +141,22 @@ namespace OBBRSS_fit_functions { //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit1(const Vector3d* const ps, OBBRSS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit2(const Vector3d* const ps, OBBRSS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fit3(const Vector3d* const ps, OBBRSS& bv); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void fitn(const Vector3d* const ps, int n, OBBRSS& bv); //============================================================================== @@ -164,55 +165,55 @@ void fitn(const Vector3d* const ps, int n, OBBRSS& bv); //============================================================================== template -struct FCL_INSTANTIATION_DEF_API Fitter>; +struct FCL_EXPORT Fitter>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API Fitter>; +struct FCL_EXPORT Fitter>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API Fitter>; +struct FCL_EXPORT Fitter>; //============================================================================== template -struct FCL_INSTANTIATION_DEF_API Fitter>; +struct FCL_EXPORT Fitter>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API ConvertBVImpl, AABB>; +class FCL_EXPORT ConvertBVImpl, AABB>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API ConvertBVImpl, OBB>; +class FCL_EXPORT ConvertBVImpl, OBB>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API ConvertBVImpl, OBB>; +class FCL_EXPORT ConvertBVImpl, OBB>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API ConvertBVImpl, OBB>; +class FCL_EXPORT ConvertBVImpl, OBB>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API ConvertBVImpl, OBB>; +class FCL_EXPORT ConvertBVImpl, OBB>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API ConvertBVImpl, RSS>; +class FCL_EXPORT ConvertBVImpl, RSS>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API ConvertBVImpl, RSS>; +class FCL_EXPORT ConvertBVImpl, RSS>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API ConvertBVImpl, RSS>; +class FCL_EXPORT ConvertBVImpl, RSS>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API ConvertBVImpl, RSS>; +class FCL_EXPORT ConvertBVImpl, RSS>; } // namespace detail } // namespace fcl diff --git a/src/math/detail/polysolver.cpp b/src/math/detail/polysolver.cpp index ed2f1ce78..b812d5415 100644 --- a/src/math/detail/polysolver.cpp +++ b/src/math/detail/polysolver.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_MATH_DETAIL_POLYSOLVER_BUILDING #include "fcl/math/detail/polysolver-inl.h" namespace fcl @@ -43,7 +44,7 @@ namespace fcl namespace detail { template -class FCL_INSTANTIATION_DEF_API PolySolver; +class FCL_EXPORT PolySolver; } // namespace detail } // namespace fcl diff --git a/src/math/detail/project.cpp b/src/math/detail/project.cpp index 9024d2efb..b9fad0474 100644 --- a/src/math/detail/project.cpp +++ b/src/math/detail/project.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_MATH_DETAIL_PROJECT_BUILDING #include "fcl/math/detail/project-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -class FCL_INSTANTIATION_DEF_API Project; +class FCL_EXPORT Project; } // namespace detail } // namespace fcl diff --git a/src/math/geometry.cpp b/src/math/geometry.cpp index e2a7d3ad6..751279b99 100644 --- a/src/math/geometry.cpp +++ b/src/math/geometry.cpp @@ -35,51 +35,52 @@ /** @author Jia Pan */ +#define FCL_MATH_GEOMETRY_BUILDING #include "fcl/math/geometry-inl.h" namespace fcl { //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void normalize(Vector3d& v, bool* signal); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void hat(Matrix3d& mat, const Vector3d& vec); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void eigen(const Matrix3d& m, Vector3d& dout, Matrix3d& vout); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void eigen_old(const Matrix3d& m, Vector3d& dout, Matrix3d& vout); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void axisFromEigen( const Matrix3d& eigenV, const Vector3d& eigenS, Matrix3d& axis); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void axisFromEigen(const Matrix3d& eigenV, const Vector3d& eigenS, Transform3d& tf); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT Matrix3d generateCoordinateSystem(const Vector3d& x_axis); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void getRadiusAndOriginAndRectangleSize( const Vector3d* const ps, const Vector3d* const ps2, @@ -93,7 +94,7 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void getRadiusAndOriginAndRectangleSize( const Vector3d* const ps, const Vector3d* const ps2, @@ -106,7 +107,7 @@ void getRadiusAndOriginAndRectangleSize( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void circumCircleComputation( const Vector3d& a, const Vector3d& b, @@ -116,7 +117,7 @@ void circumCircleComputation( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double maximumDistance( const Vector3d* const ps, const Vector3d* const ps2, @@ -127,7 +128,7 @@ double maximumDistance( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void getExtentAndCenter( const Vector3d* const ps, const Vector3d* const ps2, @@ -140,7 +141,7 @@ void getExtentAndCenter( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void getCovariance( const Vector3d* const ps, const Vector3d* const ps2, @@ -154,7 +155,7 @@ namespace detail { //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double maximumDistance_mesh( const Vector3d* const ps, const Vector3d* const ps2, @@ -165,7 +166,7 @@ double maximumDistance_mesh( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double maximumDistance_pointcloud( const Vector3d* const ps, const Vector3d* const ps2, @@ -175,7 +176,7 @@ double maximumDistance_pointcloud( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void getExtentAndCenter_pointcloud( const Vector3d* const ps, const Vector3d* const ps2, @@ -187,7 +188,7 @@ void getExtentAndCenter_pointcloud( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void getExtentAndCenter_mesh( const Vector3d* const ps, const Vector3d* const ps2, diff --git a/src/math/motion/interp_motion.cpp b/src/math/motion/interp_motion.cpp index debade816..3d4c2838a 100644 --- a/src/math/motion/interp_motion.cpp +++ b/src/math/motion/interp_motion.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_MOTION_INTERP_MOTION_BUILDING #include "fcl/math/motion/interp_motion-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API InterpMotion; +class FCL_EXPORT InterpMotion; } // namespace fcl diff --git a/src/math/motion/motion_base.cpp b/src/math/motion/motion_base.cpp index 1c21cdfd0..fb1bb0ee6 100644 --- a/src/math/motion/motion_base.cpp +++ b/src/math/motion/motion_base.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_MOTION_BASE_BUILDING #include "fcl/math/motion/motion_base-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API MotionBase; +class FCL_EXPORT MotionBase; } // namespace fcl diff --git a/src/math/motion/screw_motion.cpp b/src/math/motion/screw_motion.cpp index 9958e8f7f..3a80f8579 100644 --- a/src/math/motion/screw_motion.cpp +++ b/src/math/motion/screw_motion.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_MOTION_SCREW_MOTION_BUILDING #include "fcl/math/motion/screw_motion-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API ScrewMotion; +class FCL_EXPORT ScrewMotion; } // namespace fcl diff --git a/src/math/motion/spline_motion.cpp b/src/math/motion/spline_motion.cpp index 983d82516..66a07740c 100644 --- a/src/math/motion/spline_motion.cpp +++ b/src/math/motion/spline_motion.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_MOTION_SPLINE_MOTION_BUILDING #include "fcl/math/motion/spline_motion-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SplineMotion; +class FCL_EXPORT SplineMotion; } // namespace fcl diff --git a/src/math/motion/taylor_model/interval.cpp b/src/math/motion/taylor_model/interval.cpp index b7c7932de..380995867 100644 --- a/src/math/motion/taylor_model/interval.cpp +++ b/src/math/motion/taylor_model/interval.cpp @@ -36,6 +36,7 @@ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** @author Jia Pan */ +#define FCL_MATH_MOTION_TAYLOR_MODEL_INTERVAL_BUILDING #include "fcl/math/motion/taylor_model/interval-inl.h" namespace fcl @@ -43,16 +44,16 @@ namespace fcl //============================================================================== template -struct FCL_INSTANTIATION_DEF_API Interval; +struct FCL_EXPORT Interval; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT Interval bound(const Interval& i, double v); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT Interval bound(const Interval& i, const Interval& other); } // namespace fcl diff --git a/src/math/motion/taylor_model/interval_matrix.cpp b/src/math/motion/taylor_model/interval_matrix.cpp index e2a2d18fe..02f2dd85d 100644 --- a/src/math/motion/taylor_model/interval_matrix.cpp +++ b/src/math/motion/taylor_model/interval_matrix.cpp @@ -35,6 +35,7 @@ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** @author Jia Pan */ +#define FCL_MATH_MOTION_TAYLOR_MODEL_INTERVAL_MATRIX_BUILDING #include "fcl/math/motion/taylor_model/interval_matrix-inl.h" namespace fcl @@ -42,11 +43,11 @@ namespace fcl //============================================================================== template -struct FCL_INSTANTIATION_DEF_API IMatrix3; +struct FCL_EXPORT IMatrix3; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT IMatrix3 rotationConstrain(const IMatrix3& m); } // namespace fcl diff --git a/src/math/motion/taylor_model/interval_vector.cpp b/src/math/motion/taylor_model/interval_vector.cpp index d841c95d5..a225c3664 100644 --- a/src/math/motion/taylor_model/interval_vector.cpp +++ b/src/math/motion/taylor_model/interval_vector.cpp @@ -35,6 +35,7 @@ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** @author Jia Pan */ +#define FCL_MATH_MOTION_TAYLOR_MODEL_INTERVAL_VECTOR_BUILDING #include "fcl/math/motion/taylor_model/interval_vector-inl.h" namespace fcl @@ -42,16 +43,16 @@ namespace fcl //============================================================================== template -struct FCL_INSTANTIATION_DEF_API IVector3; +struct FCL_EXPORT IVector3; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT IVector3 bound(const IVector3& i, const Vector3& v); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT IVector3 bound(const IVector3& i, const IVector3& v); } // namespace fcl diff --git a/src/math/motion/taylor_model/taylor_matrix.cpp b/src/math/motion/taylor_model/taylor_matrix.cpp index d9e0db9c7..ce206ff61 100644 --- a/src/math/motion/taylor_model/taylor_matrix.cpp +++ b/src/math/motion/taylor_model/taylor_matrix.cpp @@ -35,6 +35,7 @@ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** @author Jia Pan */ +#define FCL_MATH_MOTION_TAYLOR_MODEL_TAYLOR_MATRIX_BUILDING #include "fcl/math/motion/taylor_model/taylor_matrix-inl.h" namespace fcl @@ -42,41 +43,41 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API TMatrix3; +class FCL_EXPORT TMatrix3; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TMatrix3 rotationConstrain(const TMatrix3& m); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TMatrix3 operator * (const Matrix3& m, const TaylorModel& a); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TMatrix3 operator * (const TaylorModel& a, const Matrix3& m); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TMatrix3 operator * (const TaylorModel& a, const TMatrix3& m); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TMatrix3 operator * (double d, const TMatrix3& m); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TMatrix3 operator + (const Matrix3& m1, const TMatrix3& m2); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TMatrix3 operator - (const Matrix3& m1, const TMatrix3& m2); } // namespace fcl diff --git a/src/math/motion/taylor_model/taylor_model.cpp b/src/math/motion/taylor_model/taylor_model.cpp index fb1a4caf9..8273076c1 100644 --- a/src/math/motion/taylor_model/taylor_model.cpp +++ b/src/math/motion/taylor_model/taylor_model.cpp @@ -38,6 +38,7 @@ /** @author Jia Pan */ +#define FCL_MATH_MOTION_TAYLOR_MODEL_BUILDING #include "fcl/math/motion/taylor_model/taylor_model-inl.h" namespace fcl @@ -45,36 +46,36 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API TaylorModel; +class FCL_EXPORT TaylorModel; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TaylorModel operator * (double d, const TaylorModel& a); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TaylorModel operator + (double d, const TaylorModel& a); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TaylorModel operator - (double d, const TaylorModel& a); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void generateTaylorModelForCosFunc(TaylorModel& tm, double w, double q0); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void generateTaylorModelForSinFunc(TaylorModel& tm, double w, double q0); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void generateTaylorModelForLinearFunc(TaylorModel& tm, double p, double v); } // namespace fcl diff --git a/src/math/motion/taylor_model/taylor_vector.cpp b/src/math/motion/taylor_model/taylor_vector.cpp index bc22223dc..e8be33956 100644 --- a/src/math/motion/taylor_model/taylor_vector.cpp +++ b/src/math/motion/taylor_model/taylor_vector.cpp @@ -35,6 +35,7 @@ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** @author Jia Pan */ +#define FCL_MATH_MOTION_TAYLOR_MODEL_TAYLOR_VECTOR_BUILDING #include "fcl/math/motion/taylor_model/taylor_vector-inl.h" namespace fcl @@ -42,26 +43,26 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API TVector3; +class FCL_EXPORT TVector3; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void generateTVector3ForLinearFunc(TVector3& v, const Vector3& position, const Vector3& velocity); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TVector3 operator * (const Vector3& v, const TaylorModel& a); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TVector3 operator + (const Vector3& v1, const TVector3& v2); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT TVector3 operator - (const Vector3& v1, const TVector3& v2); } // namespace fcl diff --git a/src/math/motion/taylor_model/time_interval.cpp b/src/math/motion/taylor_model/time_interval.cpp index 262afe58e..163e67561 100644 --- a/src/math/motion/taylor_model/time_interval.cpp +++ b/src/math/motion/taylor_model/time_interval.cpp @@ -35,12 +35,13 @@ // This code is based on code developed by Stephane Redon at UNC and Inria for the CATCH library: http://graphics.ewha.ac.kr/CATCH/ /** @author Jia Pan */ +#define FCL_MATH_MOTION_TAYLOR_MODEL_TIME_INTERVAL_BUILDING #include "fcl/math/motion/taylor_model/time_interval-inl.h" namespace fcl { template -struct FCL_INSTANTIATION_DEF_API TimeInterval; +struct FCL_EXPORT TimeInterval; } // namespace fcl diff --git a/src/math/motion/translation_motion.cpp b/src/math/motion/translation_motion.cpp index 26e1ba7cb..459501bfb 100644 --- a/src/math/motion/translation_motion.cpp +++ b/src/math/motion/translation_motion.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_MOTION_TRANSLATION_MOTION_BUILDING #include "fcl/math/motion/translation_motion-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API TranslationMotion; +class FCL_EXPORT TranslationMotion; } // namespace fcl diff --git a/src/math/motion/triangle_motion_bound_visitor.cpp b/src/math/motion/triangle_motion_bound_visitor.cpp index f7e237a25..652ded42d 100644 --- a/src/math/motion/triangle_motion_bound_visitor.cpp +++ b/src/math/motion/triangle_motion_bound_visitor.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_MOTION_TRIANGLE_MOTION_BOUND_VISITOR_BUILDING #include "fcl/math/motion/triangle_motion_bound_visitor-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API TriangleMotionBoundVisitor; +class FCL_EXPORT TriangleMotionBoundVisitor; } // namespace fcl diff --git a/src/math/rng.cpp b/src/math/rng.cpp index 34b7864cd..d319b764d 100644 --- a/src/math/rng.cpp +++ b/src/math/rng.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_MATH_RNG_BUILDING #include "fcl/math/rng-inl.h" namespace fcl @@ -42,6 +43,6 @@ namespace fcl //============================================================================== template -class FCL_INSTANTIATION_DEF_API RNG; +class FCL_EXPORT RNG; } // namespace fcl diff --git a/src/math/sampler/sampler_base.cpp b/src/math/sampler/sampler_base.cpp index 5291007f8..1cd16d319 100644 --- a/src/math/sampler/sampler_base.cpp +++ b/src/math/sampler/sampler_base.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_SAMPLER_BASE_BUILDING #include "fcl/math/sampler/sampler_base.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SamplerBase; +class FCL_EXPORT SamplerBase; } // namespace fcl diff --git a/src/math/sampler/sampler_se2.cpp b/src/math/sampler/sampler_se2.cpp index 104b255d7..a692f1b20 100644 --- a/src/math/sampler/sampler_se2.cpp +++ b/src/math/sampler/sampler_se2.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_SAMPLER_SE2_BUILDING #include "fcl/math/sampler/sampler_se2-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SamplerSE2; +class FCL_EXPORT SamplerSE2; } // namespace fcl diff --git a/src/math/sampler/sampler_se2_disk.cpp b/src/math/sampler/sampler_se2_disk.cpp index ba0cad1bf..f9d84d55c 100644 --- a/src/math/sampler/sampler_se2_disk.cpp +++ b/src/math/sampler/sampler_se2_disk.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_SAMPLER_SE2_DISK_BUILDING #include "fcl/math/sampler/sampler_se2_disk-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SamplerSE2_disk; +class FCL_EXPORT SamplerSE2_disk; } // namespace fcl diff --git a/src/math/sampler/sampler_se3_euler.cpp b/src/math/sampler/sampler_se3_euler.cpp index fe5697abf..f251f5c3e 100644 --- a/src/math/sampler/sampler_se3_euler.cpp +++ b/src/math/sampler/sampler_se3_euler.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_SAMPLER_SE3_EULER_BUILDING #include "fcl/math/sampler/sampler_se3_euler-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SamplerSE3Euler; +class FCL_EXPORT SamplerSE3Euler; } // namespace fcl diff --git a/src/math/sampler/sampler_se3_euler_ball.cpp b/src/math/sampler/sampler_se3_euler_ball.cpp index 22257b8b9..9a1eba122 100644 --- a/src/math/sampler/sampler_se3_euler_ball.cpp +++ b/src/math/sampler/sampler_se3_euler_ball.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_SAMPLER_SE3_EULER_BALL_BUILDING #include "fcl/math/sampler/sampler_se3_euler_ball-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SamplerSE3Euler_ball; +class FCL_EXPORT SamplerSE3Euler_ball; } // namespace fcl diff --git a/src/math/sampler/sampler_se3_quat.cpp b/src/math/sampler/sampler_se3_quat.cpp index 966211bf3..ef7f197d1 100644 --- a/src/math/sampler/sampler_se3_quat.cpp +++ b/src/math/sampler/sampler_se3_quat.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_SAMPLER_SE3_QUAT_BUILDING #include "fcl/math/sampler/sampler_se3_quat-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SamplerSE3Quat; +class FCL_EXPORT SamplerSE3Quat; } // namespace fcl diff --git a/src/math/sampler/sampler_se3_quat_ball.cpp b/src/math/sampler/sampler_se3_quat_ball.cpp index 78463929e..6c4689f11 100644 --- a/src/math/sampler/sampler_se3_quat_ball.cpp +++ b/src/math/sampler/sampler_se3_quat_ball.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_SAMPLER_SE3_QUAT_BALL_BUILDING #include "fcl/math/sampler/sampler_se3_quat_ball-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API SamplerSE3Quat_ball; +class FCL_EXPORT SamplerSE3Quat_ball; } // namespace fcl diff --git a/src/math/variance3.cpp b/src/math/variance3.cpp index ce08cbc91..1a361d0e2 100644 --- a/src/math/variance3.cpp +++ b/src/math/variance3.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_MATH_VARIANCE3_BUILDING #include "fcl/math/variance3-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API Variance3; +class FCL_EXPORT Variance3; } // namespace fcl diff --git a/src/narrowphase/collision.cpp b/src/narrowphase/collision.cpp index 70067835f..fb574947d 100644 --- a/src/narrowphase/collision.cpp +++ b/src/narrowphase/collision.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_COLLISION_BUILDING #include "fcl/narrowphase/collision-inl.h" namespace fcl @@ -42,7 +43,7 @@ namespace fcl //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT std::size_t collide( const CollisionObject* o1, const CollisionObject* o2, @@ -51,7 +52,7 @@ std::size_t collide( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT std::size_t collide( const CollisionGeometry* o1, const Transform3& tf1, diff --git a/src/narrowphase/collision_object.cpp b/src/narrowphase/collision_object.cpp index 97d4e53dc..cf7011707 100644 --- a/src/narrowphase/collision_object.cpp +++ b/src/narrowphase/collision_object.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_COLLISION_OBJECT_BUILDING #include "fcl/narrowphase/collision_object-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API CollisionObject; +class FCL_EXPORT CollisionObject; } // namespace fcl diff --git a/src/narrowphase/collision_request.cpp b/src/narrowphase/collision_request.cpp index 372d8abe4..0537d8963 100644 --- a/src/narrowphase/collision_request.cpp +++ b/src/narrowphase/collision_request.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_COLLISION_REQUEST_BUILDING #include "fcl/narrowphase/collision_request-inl.h" namespace fcl { template -struct FCL_INSTANTIATION_DEF_API CollisionRequest; +struct FCL_EXPORT CollisionRequest; } // namespace fcl diff --git a/src/narrowphase/collision_result.cpp b/src/narrowphase/collision_result.cpp index 02403cb40..c2fad791b 100644 --- a/src/narrowphase/collision_result.cpp +++ b/src/narrowphase/collision_result.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_COLLISION_RESULT_BUILDING #include "fcl/narrowphase/collision_result-inl.h" namespace fcl { template -struct FCL_INSTANTIATION_DEF_API CollisionResult; +struct FCL_EXPORT CollisionResult; } // namespace fcl diff --git a/src/narrowphase/contact.cpp b/src/narrowphase/contact.cpp index 9e45c61ad..9ab961e08 100644 --- a/src/narrowphase/contact.cpp +++ b/src/narrowphase/contact.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_CONTACT_BUILDING #include "fcl/narrowphase/contact-inl.h" namespace fcl { template -struct FCL_INSTANTIATION_DEF_API Contact; +struct FCL_EXPORT Contact; } // namespace fcl diff --git a/src/narrowphase/contact_point.cpp b/src/narrowphase/contact_point.cpp index 0c2253fa4..276d16cfa 100644 --- a/src/narrowphase/contact_point.cpp +++ b/src/narrowphase/contact_point.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_CONTACT_POINT_BUILDING #include "fcl/narrowphase/contact_point-inl.h" namespace fcl @@ -42,17 +43,17 @@ namespace fcl //============================================================================== template -struct FCL_INSTANTIATION_DEF_API ContactPoint; +struct FCL_EXPORT ContactPoint; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool comparePenDepth( const ContactPoint& _cp1, const ContactPoint& _cp2); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void flipNormal(std::vector>& contacts); } // namespace fcl diff --git a/src/narrowphase/continuous_collision.cpp b/src/narrowphase/continuous_collision.cpp index c62ebcdea..866d86865 100644 --- a/src/narrowphase/continuous_collision.cpp +++ b/src/narrowphase/continuous_collision.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_CONTINUOUS_COLLISION_BUILDING #include "fcl/narrowphase/continuous_collision-inl.h" namespace fcl @@ -42,7 +43,7 @@ namespace fcl //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double continuousCollide( const CollisionGeometry* o1, const MotionBase* motion1, @@ -53,7 +54,7 @@ double continuousCollide( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double continuousCollide( const CollisionGeometry* o1, const Transform3& tf1_beg, @@ -66,7 +67,7 @@ double continuousCollide( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double continuousCollide( const CollisionObject* o1, const Transform3& tf1_end, @@ -77,7 +78,7 @@ double continuousCollide( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double collide( const ContinuousCollisionObject* o1, const ContinuousCollisionObject* o2, diff --git a/src/narrowphase/continuous_collision_object.cpp b/src/narrowphase/continuous_collision_object.cpp index 8719cb900..3b99c9c06 100644 --- a/src/narrowphase/continuous_collision_object.cpp +++ b/src/narrowphase/continuous_collision_object.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_CONTINUOUS_COLLISION_OBJECT_BUILDING #include "fcl/narrowphase/continuous_collision_object-inl.h" namespace fcl { template -class FCL_INSTANTIATION_DEF_API ContinuousCollisionObject; +class FCL_EXPORT ContinuousCollisionObject; } // namespace fcl diff --git a/src/narrowphase/continuous_collision_request.cpp b/src/narrowphase/continuous_collision_request.cpp index 50042dc15..36fa7f0e2 100644 --- a/src/narrowphase/continuous_collision_request.cpp +++ b/src/narrowphase/continuous_collision_request.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_CONTINUOUS_COLLISION_REQUEST_BUILDING #include "fcl/narrowphase/continuous_collision_request-inl.h" namespace fcl { template -struct FCL_INSTANTIATION_DEF_API ContinuousCollisionRequest; +struct FCL_EXPORT ContinuousCollisionRequest; } // namespace fcl diff --git a/src/narrowphase/continuous_collision_result.cpp b/src/narrowphase/continuous_collision_result.cpp index e6a203191..126c46a80 100644 --- a/src/narrowphase/continuous_collision_result.cpp +++ b/src/narrowphase/continuous_collision_result.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_CONTINUOUS_COLLISION_RESULT_BUILDING #include "fcl/narrowphase/continuous_collision_result-inl.h" namespace fcl { template -struct FCL_INSTANTIATION_DEF_API ContinuousCollisionResult; +struct FCL_EXPORT ContinuousCollisionResult; } // namespace fcl diff --git a/src/narrowphase/cost_source.cpp b/src/narrowphase/cost_source.cpp index 482b93896..d14560da3 100644 --- a/src/narrowphase/cost_source.cpp +++ b/src/narrowphase/cost_source.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_COST_SOURCE_BUILDING #include "fcl/narrowphase/cost_source-inl.h" namespace fcl { template -struct FCL_INSTANTIATION_DEF_API CostSource; +struct FCL_EXPORT CostSource; } // namespace fcl diff --git a/src/narrowphase/detail/convexity_based_algorithm/epa.cpp b/src/narrowphase/detail/convexity_based_algorithm/epa.cpp index 136851b3a..7132f294f 100644 --- a/src/narrowphase/detail/convexity_based_algorithm/epa.cpp +++ b/src/narrowphase/detail/convexity_based_algorithm/epa.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_CONVEXITY_BASED_ALGORITHM_EPA_BUILDING #include "fcl/narrowphase/detail/convexity_based_algorithm/epa-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API EPA; +struct FCL_EXPORT EPA; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/convexity_based_algorithm/gjk.cpp b/src/narrowphase/detail/convexity_based_algorithm/gjk.cpp index 4cb1747f0..b5373106c 100644 --- a/src/narrowphase/detail/convexity_based_algorithm/gjk.cpp +++ b/src/narrowphase/detail/convexity_based_algorithm/gjk.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_CONVEXITY_BASED_ALGORITHM_GJK_BUILDING #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API GJK; +struct FCL_EXPORT GJK; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/convexity_based_algorithm/gjk_libccd.cpp b/src/narrowphase/detail/convexity_based_algorithm/gjk_libccd.cpp index a9592c8e7..d2853dd6c 100644 --- a/src/narrowphase/detail/convexity_based_algorithm/gjk_libccd.cpp +++ b/src/narrowphase/detail/convexity_based_algorithm/gjk_libccd.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_CONVEXITY_BASED_ALGORITHM_GJK_LIBCCD_BUILDING #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h" namespace fcl @@ -45,41 +46,41 @@ namespace detail //============================================================================== template -class FCL_INSTANTIATION_DEF_API GJKInitializer>; +class FCL_EXPORT GJKInitializer>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API GJKInitializer>; +class FCL_EXPORT GJKInitializer>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API GJKInitializer>; +class FCL_EXPORT GJKInitializer>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API GJKInitializer>; +class FCL_EXPORT GJKInitializer>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API GJKInitializer>; +class FCL_EXPORT GJKInitializer>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API GJKInitializer>; +class FCL_EXPORT GJKInitializer>; //============================================================================== template -class FCL_INSTANTIATION_DEF_API GJKInitializer>; +class FCL_EXPORT GJKInitializer>; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void* triCreateGJKObject( const Vector3d& P1, const Vector3d& P2, const Vector3d& P3); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void* triCreateGJKObject( const Vector3d& P1, const Vector3d& P2, @@ -88,7 +89,7 @@ void* triCreateGJKObject( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool GJKCollide( void* obj1, ccd_support_fn supp1, @@ -104,7 +105,7 @@ bool GJKCollide( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool GJKDistance( void* obj1, ccd_support_fn supp1, @@ -117,7 +118,7 @@ bool GJKDistance( Vector3d* p2); template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool GJKSignedDistance( void* obj1, ccd_support_fn supp1, diff --git a/src/narrowphase/detail/convexity_based_algorithm/minkowski_diff.cpp b/src/narrowphase/detail/convexity_based_algorithm/minkowski_diff.cpp index 0f0dbb6cc..62b185f16 100644 --- a/src/narrowphase/detail/convexity_based_algorithm/minkowski_diff.cpp +++ b/src/narrowphase/detail/convexity_based_algorithm/minkowski_diff.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_CONVEXITY_BASED_ALGORITHM_MINKOWSKI_DIFF_BUILDING #include "fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API MinkowskiDiff; +struct FCL_EXPORT MinkowskiDiff; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/gjk_solver_indep.cpp b/src/narrowphase/detail/gjk_solver_indep.cpp index 6a881d090..531b57127 100755 --- a/src/narrowphase/detail/gjk_solver_indep.cpp +++ b/src/narrowphase/detail/gjk_solver_indep.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_GJK_SOLVER_INDEP_BUILDING #include "fcl/narrowphase/detail/gjk_solver_indep-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API GJKSolver_indep; +struct FCL_EXPORT GJKSolver_indep; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/gjk_solver_libccd.cpp b/src/narrowphase/detail/gjk_solver_libccd.cpp index d43cd7571..797df7f6e 100755 --- a/src/narrowphase/detail/gjk_solver_libccd.cpp +++ b/src/narrowphase/detail/gjk_solver_libccd.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_GJK_SOLVER_LIBCCD_BUILDING #include "fcl/narrowphase/detail/gjk_solver_libccd-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API GJKSolver_libccd; +struct FCL_EXPORT GJKSolver_libccd; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/box_box.cpp b/src/narrowphase/detail/primitive_shape_algorithm/box_box.cpp index 481d8ce75..6839db183 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/box_box.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/box_box.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_BOX_BOX_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/box_box-inl.h" namespace fcl @@ -45,24 +46,24 @@ namespace detail //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void lineClosestApproach(const Vector3& pa, const Vector3& ua, const Vector3& pb, const Vector3& ub, double* alpha, double* beta); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT int intersectRectQuad2(double h[2], double p[8], double ret[16]); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void cullPoints2(int n, double p[], int m, int i0, int iret[]); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT int boxBox2( const Vector3& side1, const Transform3& tf1, @@ -76,7 +77,7 @@ int boxBox2( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool boxBoxIntersect(const Box& s1, const Transform3& tf1, const Box& s2, const Transform3& tf2, std::vector>* contacts_); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.cpp b/src/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.cpp index f481b6dc4..bfc1288a4 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_CAPSULE_CAPSULE_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h" namespace fcl @@ -45,12 +46,12 @@ namespace detail //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double clamp(double n, double min, double max); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double closestPtSegmentSegment(const Vector3d& p_FP1, const Vector3d& p_FQ1, const Vector3d& p_FP2, @@ -60,7 +61,7 @@ double closestPtSegmentSegment(const Vector3d& p_FP1, //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool capsuleCapsuleDistance( const Capsule& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, diff --git a/src/narrowphase/detail/primitive_shape_algorithm/halfspace.cpp b/src/narrowphase/detail/primitive_shape_algorithm/halfspace.cpp index 6017c0985..12cd659dc 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/halfspace.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/halfspace.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_HALFSPACE_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/halfspace-inl.h" namespace fcl @@ -59,7 +60,7 @@ double halfspaceIntersectTolerance() //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereHalfspaceIntersect( const Sphere& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -67,7 +68,7 @@ bool sphereHalfspaceIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool ellipsoidHalfspaceIntersect( const Ellipsoid& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -75,14 +76,14 @@ bool ellipsoidHalfspaceIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool boxHalfspaceIntersect( const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool boxHalfspaceIntersect( const Box& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -90,7 +91,7 @@ bool boxHalfspaceIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool capsuleHalfspaceIntersect( const Capsule& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -98,7 +99,7 @@ bool capsuleHalfspaceIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool cylinderHalfspaceIntersect( const Cylinder& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -106,7 +107,7 @@ bool cylinderHalfspaceIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool coneHalfspaceIntersect( const Cone& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -114,7 +115,7 @@ bool coneHalfspaceIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool convexHalfspaceIntersect( const Convex& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -122,7 +123,7 @@ bool convexHalfspaceIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool convexHalfspaceIntersect(const Convex& convex_C, const Transform3& X_FC, const Halfspace& half_space_H, @@ -131,7 +132,7 @@ bool convexHalfspaceIntersect(const Convex& convex_C, //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool halfspaceTriangleIntersect( const Halfspace& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, @@ -139,7 +140,7 @@ bool halfspaceTriangleIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool planeHalfspaceIntersect( const Plane& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, @@ -150,7 +151,7 @@ bool planeHalfspaceIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool halfspacePlaneIntersect( const Halfspace& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, @@ -160,7 +161,7 @@ bool halfspacePlaneIntersect( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool halfspaceIntersect( const Halfspace& s1, const Transform3& tf1, const Halfspace& s2, const Transform3& tf2, diff --git a/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp b/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp index 094def0e0..68e412603 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_INTERSECT_BUILDING #include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -class FCL_INSTANTIATION_DEF_API Intersect; +class FCL_EXPORT Intersect; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/plane.cpp b/src/narrowphase/detail/primitive_shape_algorithm/plane.cpp index 1b246caf6..eecc16653 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/plane.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/plane.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_PLANE_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/plane-inl.h" namespace fcl @@ -59,75 +60,75 @@ float planeIntersectTolerance() //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool spherePlaneIntersect(const Sphere& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool ellipsoidPlaneIntersect(const Ellipsoid& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool boxPlaneIntersect(const Box& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool capsulePlaneIntersect(const Capsule& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool conePlaneIntersect(const Cone& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool convexPlaneIntersect(const Convex& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, Vector3* contact_points, double* penetration_depth, Vector3* normal); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool planeTriangleIntersect(const Plane& s1, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, Vector3* contact_points, double* penetration_depth, Vector3* normal); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool planeIntersect(const Plane& s1, const Transform3& tf1, const Plane& s2, const Transform3& tf2, std::vector>* contacts); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp index 34c8aa4d3..9f6641066 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_box.cpp @@ -34,6 +34,7 @@ /** @author Sean Curtis (sean@tri.global) (2018) */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_BOX_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereBoxIntersect(const Sphere& sphere, const Transform3& X_FS, const Box& box, const Transform3& X_FB, std::vector>* contacts); @@ -52,7 +53,7 @@ bool sphereBoxIntersect(const Sphere& sphere, const Transform3& //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereBoxDistance(const Sphere& sphere, const Transform3& X_FS, const Box& box, const Transform3& X_FB, double* distance, Vector3* p_FSb, diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.cpp index 372d94599..f1c3d3a45 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_capsule.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_CAPSULE_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_capsule-inl.h" namespace fcl @@ -45,7 +46,7 @@ namespace detail //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void lineSegmentPointClosestToPoint( const Vector3 &p, const Vector3 &s1, @@ -54,14 +55,14 @@ void lineSegmentPointClosestToPoint( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereCapsuleIntersect(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereCapsuleDistance(const Sphere& s1, const Transform3& tf1, const Capsule& s2, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp index 0d663b1e5..04cbcfcd9 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder.cpp @@ -34,6 +34,7 @@ /** @author Sean Curtis (sean@tri.global) (2018) */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_CYLINDER_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h" namespace fcl @@ -43,7 +44,7 @@ namespace detail { template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereCylinderIntersect(const Sphere& sphere, const Transform3& X_FS, const Cylinder& cylinder, @@ -53,7 +54,7 @@ bool sphereCylinderIntersect(const Sphere& sphere, //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereCylinderDistance(const Sphere& sphere, const Transform3& X_FS, const Cylinder& cylinder, diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.cpp index df2d0506f..2c160a55d 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_sphere.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_SPHERE_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_sphere-inl.h" namespace fcl @@ -45,14 +46,14 @@ namespace detail //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereSphereIntersect(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, std::vector>* contacts); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereSphereDistance(const Sphere& s1, const Transform3& tf1, const Sphere& s2, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.cpp b/src/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.cpp index 2a1fb5d1c..60481751f 100755 --- a/src/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/sphere_triangle.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_SPHERE_TRIANGLE_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/sphere_triangle-inl.h" namespace fcl @@ -45,37 +46,37 @@ namespace detail //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double segmentSqrDistance(const Vector3& from, const Vector3& to,const Vector3& p, Vector3& nearest); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool projectInTriangle(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& normal, const Vector3& p); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereTriangleIntersect(const Sphere& s, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, Vector3* contact_points, double* penetration_depth, Vector3* normal_); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, double* dist); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf, const Vector3& P1, const Vector3& P2, const Vector3& P3, double* dist, Vector3* p1, Vector3* p2); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool sphereTriangleDistance(const Sphere& sp, const Transform3& tf1, const Vector3& P1, const Vector3& P2, const Vector3& P3, const Transform3& tf2, double* dist, Vector3* p1, Vector3* p2); diff --git a/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp b/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp index abaeb602a..3f49f7d65 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_PRIMITIVE_SHAPE_ALGORITHM_TRIANGLE_DISTANCE_BUILDING #include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" namespace fcl @@ -45,7 +46,7 @@ namespace detail //============================================================================== template -class FCL_INSTANTIATION_DEF_API TriangleDistance; +class FCL_EXPORT TriangleDistance; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/collision/collision_traversal_node_base.cpp b/src/narrowphase/detail/traversal/collision/collision_traversal_node_base.cpp index adf3e1b4f..b6e879ee9 100644 --- a/src/narrowphase/detail/traversal/collision/collision_traversal_node_base.cpp +++ b/src/narrowphase/detail/traversal/collision/collision_traversal_node_base.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_TRAVERSAL_NODE_BASE_BUILDING #include "fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -class FCL_INSTANTIATION_DEF_API CollisionTraversalNodeBase; +class FCL_EXPORT CollisionTraversalNodeBase; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.cpp b/src/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.cpp index a4dcd3c38..430b1bf53 100644 --- a/src/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.cpp +++ b/src/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_MESH_COLLISION_TRAVERSAL_NODE_BUILDING #include "fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node-inl.h" namespace fcl @@ -45,11 +46,11 @@ namespace detail //============================================================================== template -class FCL_INSTANTIATION_DEF_API MeshCollisionTraversalNodeOBB; +class FCL_EXPORT MeshCollisionTraversalNodeOBB; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool initialize( MeshCollisionTraversalNodeOBB& node, const BVHModel>& model1, @@ -61,11 +62,11 @@ bool initialize( //============================================================================== template -class FCL_INSTANTIATION_DEF_API MeshCollisionTraversalNodeRSS; +class FCL_EXPORT MeshCollisionTraversalNodeRSS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool initialize( MeshCollisionTraversalNodeRSS& node, const BVHModel>& model1, @@ -77,11 +78,11 @@ bool initialize( //============================================================================== template -class FCL_INSTANTIATION_DEF_API MeshCollisionTraversalNodekIOS; +class FCL_EXPORT MeshCollisionTraversalNodekIOS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool initialize( MeshCollisionTraversalNodekIOS& node, const BVHModel>& model1, @@ -93,11 +94,11 @@ bool initialize( //============================================================================== template -class FCL_INSTANTIATION_DEF_API MeshCollisionTraversalNodeOBBRSS; +class FCL_EXPORT MeshCollisionTraversalNodeOBBRSS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool initialize( MeshCollisionTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/src/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.cpp b/src/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.cpp index 760ca025c..d1190e049 100644 --- a/src/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.cpp +++ b/src/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_MESH_CONTINUOUS_COLLISION_TRAVERSAL_NODE_BUILDING #include "fcl/narrowphase/detail/traversal/collision/mesh_continuous_collision_traversal_node-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API BVHContinuousCollisionPair; +struct FCL_EXPORT BVHContinuousCollisionPair; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/collision_node.cpp b/src/narrowphase/detail/traversal/collision_node.cpp index 0fa989540..8514d7ba1 100644 --- a/src/narrowphase/detail/traversal/collision_node.cpp +++ b/src/narrowphase/detail/traversal/collision_node.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_COLLISION_NODE_BUILDING #include "fcl/narrowphase/detail/traversal/collision_node-inl.h" namespace fcl @@ -45,27 +46,27 @@ namespace detail //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list, int qsize); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list); } // namespace detail diff --git a/src/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.cpp b/src/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.cpp index 174306910..228140468 100644 --- a/src/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.cpp +++ b/src/narrowphase/detail/traversal/distance/conservative_advancement_stack_data.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_DISTANCE_CONSERVATIVE_ADVANCEMENT_STACK_DATA_BUILDING #include "fcl/narrowphase/detail/traversal/distance/conservative_advancement_stack_data-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -struct FCL_INSTANTIATION_DEF_API ConservativeAdvancementStackData; +struct FCL_EXPORT ConservativeAdvancementStackData; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/distance/distance_traversal_node_base.cpp b/src/narrowphase/detail/traversal/distance/distance_traversal_node_base.cpp index ab2859c7d..49cadbac1 100644 --- a/src/narrowphase/detail/traversal/distance/distance_traversal_node_base.cpp +++ b/src/narrowphase/detail/traversal/distance/distance_traversal_node_base.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_DISTANCE_TRAVERSAL_NODE_BASE_BUILDING #include "fcl/narrowphase/detail/traversal/distance/distance_traversal_node_base-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -class FCL_INSTANTIATION_DEF_API DistanceTraversalNodeBase; +class FCL_EXPORT DistanceTraversalNodeBase; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.cpp b/src/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.cpp index 72018ca30..ae1a8a119 100644 --- a/src/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.cpp +++ b/src/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_DISTANCE_MESH_CONSERVATIVE_ADVANCEMENT_TRAVERSAL_NODE_BUILDING #include "fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h" namespace fcl @@ -45,11 +46,11 @@ namespace detail //============================================================================== template -class FCL_INSTANTIATION_DEF_API MeshConservativeAdvancementTraversalNodeRSS; +class FCL_EXPORT MeshConservativeAdvancementTraversalNodeRSS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool initialize( MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel>& model1, @@ -60,11 +61,11 @@ bool initialize( //============================================================================== template -class FCL_INSTANTIATION_DEF_API MeshConservativeAdvancementTraversalNodeOBBRSS; +class FCL_EXPORT MeshConservativeAdvancementTraversalNodeOBBRSS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool initialize( MeshConservativeAdvancementTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/src/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.cpp b/src/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.cpp index d27885a21..ef5808460 100644 --- a/src/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.cpp +++ b/src/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_DISTANCE_MESH_DISTANCE_TRAVERSAL_NODE_BUILDING #include "fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node-inl.h" namespace fcl @@ -45,11 +46,11 @@ namespace detail //============================================================================== template -class FCL_INSTANTIATION_DEF_API MeshDistanceTraversalNodeRSS; +class FCL_EXPORT MeshDistanceTraversalNodeRSS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool initialize( MeshDistanceTraversalNodeRSS& node, const BVHModel>& model1, @@ -61,11 +62,11 @@ bool initialize( //============================================================================== template -class FCL_INSTANTIATION_DEF_API MeshDistanceTraversalNodekIOS; +class FCL_EXPORT MeshDistanceTraversalNodekIOS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool initialize( MeshDistanceTraversalNodekIOS& node, const BVHModel>& model1, @@ -77,11 +78,11 @@ bool initialize( //============================================================================== template -class FCL_INSTANTIATION_DEF_API MeshDistanceTraversalNodeOBBRSS; +class FCL_EXPORT MeshDistanceTraversalNodeOBBRSS; //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT bool initialize( MeshDistanceTraversalNodeOBBRSS& node, const BVHModel>& model1, diff --git a/src/narrowphase/detail/traversal/traversal_node_base.cpp b/src/narrowphase/detail/traversal/traversal_node_base.cpp index 8875b212a..6b7aef288 100644 --- a/src/narrowphase/detail/traversal/traversal_node_base.cpp +++ b/src/narrowphase/detail/traversal/traversal_node_base.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_NODE_BASE_BUILDING #include "fcl/narrowphase/detail/traversal/traversal_node_base-inl.h" namespace fcl @@ -44,7 +45,7 @@ namespace detail { template -class FCL_INSTANTIATION_DEF_API TraversalNodeBase; +class FCL_EXPORT TraversalNodeBase; } // namespace detail } // namespace fcl diff --git a/src/narrowphase/detail/traversal/traversal_recurse.cpp b/src/narrowphase/detail/traversal/traversal_recurse.cpp index 146a7851a..2a83d4f14 100644 --- a/src/narrowphase/detail/traversal/traversal_recurse.cpp +++ b/src/narrowphase/detail/traversal/traversal_recurse.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DETAIL_TRAVERSAL_RECURSE_BUILDING #include "fcl/narrowphase/detail/traversal/traversal_recurse-inl.h" namespace fcl @@ -45,37 +46,37 @@ namespace detail //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3& R, const Vector3& T, BVHFrontList* front_list); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize); //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list); } // namespace detail diff --git a/src/narrowphase/distance.cpp b/src/narrowphase/distance.cpp index 94d4a1c89..d0c7920cf 100644 --- a/src/narrowphase/distance.cpp +++ b/src/narrowphase/distance.cpp @@ -35,6 +35,7 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DISTANCE_BUILDING #include "fcl/narrowphase/distance-inl.h" namespace fcl @@ -42,7 +43,7 @@ namespace fcl //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double distance( const CollisionObject* o1, const CollisionObject* o2, @@ -51,7 +52,7 @@ double distance( //============================================================================== template -FCL_INSTANTIATION_DEF_API +FCL_EXPORT double distance( const CollisionGeometry* o1, const Transform3& tf1, const CollisionGeometry* o2, const Transform3& tf2, diff --git a/src/narrowphase/distance_request.cpp b/src/narrowphase/distance_request.cpp index f06720233..e8a34fd91 100644 --- a/src/narrowphase/distance_request.cpp +++ b/src/narrowphase/distance_request.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DISTANCE_REQUEST_BUILDING #include "fcl/narrowphase/distance_request-inl.h" namespace fcl { template -struct FCL_INSTANTIATION_DEF_API DistanceRequest; +struct FCL_EXPORT DistanceRequest; } // namespace fcl diff --git a/src/narrowphase/distance_result.cpp b/src/narrowphase/distance_result.cpp index 5171620ca..a7b4fe406 100644 --- a/src/narrowphase/distance_result.cpp +++ b/src/narrowphase/distance_result.cpp @@ -35,12 +35,13 @@ /** @author Jia Pan */ +#define FCL_NARROWPHASE_DISTANCE_RESULT_BUILDING #include "fcl/narrowphase/distance_result-inl.h" namespace fcl { template -struct FCL_INSTANTIATION_DEF_API DistanceResult; +struct FCL_EXPORT DistanceResult; } // namespace fcl