diff --git a/.travis.yml b/.travis.yml index 37310a76a..87677ea0a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,3 +1,8 @@ +sudo: required +dist: trusty +cache: + apt: true + language: cpp os: @@ -17,6 +22,12 @@ matrix: - os: osx compiler: gcc +addons: + apt: + packages: + - libflann-dev + - libboost-all-dev + install: # Install dependencies for FCL - if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/install_linux.sh' ; fi diff --git a/CMakeLists.txt b/CMakeLists.txt index abc437861..a8ac189d4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,7 +104,7 @@ endif() # endif() -find_package(Boost COMPONENTS thread date_time filesystem system unit_test_framework REQUIRED) +find_package(Boost COMPONENTS date_time filesystem system unit_test_framework REQUIRED) include_directories(${Boost_INCLUDE_DIR}) if(MSVC) @@ -145,6 +145,9 @@ add_subdirectory(src) set(pkg_conf_file_in "${CMAKE_CURRENT_SOURCE_DIR}/fcl.pc.in") set(pkg_conf_file_out "${CMAKE_CURRENT_BINARY_DIR}/fcl.pc") +if(NOT MSVC) + set(PKG_CFLAGS "-std=c++11") +endif() configure_file("${pkg_conf_file_in}" "${pkg_conf_file_out}" @ONLY) install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} diff --git a/CMakeModules/CompilerSettings.cmake b/CMakeModules/CompilerSettings.cmake index d855af0e0..2bd0f6107 100644 --- a/CMakeModules/CompilerSettings.cmake +++ b/CMakeModules/CompilerSettings.cmake @@ -1,8 +1,11 @@ +# force C++11 mode +add_definitions() + if(CMAKE_COMPILER_IS_GNUCXX) - add_definitions(-W -Wall -g -Wextra -Wno-missing-field-initializers -Wno-unused-parameter) + add_definitions(-std=c++11 -W -Wall -g -Wextra -Wno-missing-field-initializers -Wno-unused-parameter) endif(CMAKE_COMPILER_IS_GNUCXX) if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - add_definitions(-W -Wall -Wextra -Wno-missing-field-initializers -Wno-unused-parameter -Wno-delete-non-virtual-dtor -Wno-overloaded-virtual -Wno-unknown-pragmas) + add_definitions(-std=c++11 -W -Wall -Wextra -Wno-missing-field-initializers -Wno-unused-parameter -Wno-delete-non-virtual-dtor -Wno-overloaded-virtual -Wno-unknown-pragmas -Wno-deprecated-register) endif() if(MSVC OR MSVC90 OR MSVC10) @@ -15,7 +18,7 @@ else() set(IS_ICPC 0) endif() if(IS_ICPC) - add_definitions(-wd191 -wd411 -wd654 -wd1125 -wd1292 -wd1565 -wd1628 -wd2196) + add_definitions(-std=c++11 -wd191 -wd411 -wd654 -wd1125 -wd1292 -wd1565 -wd1628 -wd2196) set(CMAKE_AR "xiar" CACHE STRING "Intel archiver" FORCE) set(CMAKE_CXX_FLAGS "-pthread" CACHE STRING "Default compile flags" FORCE) set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG" @@ -31,7 +34,7 @@ else() set(IS_XLC 0) endif() if(IS_XLC) - add_definitions(-qpic -q64 -qmaxmem=-1) + add_definitions(-std=c++11 -qpic -q64 -qmaxmem=-1) set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -q64") set(CMAKE_MODULE_LINKER_FLAGS "${CMAKE_MODULE_LINKER_FLAGS} -q64") endif(IS_XLC) diff --git a/CMakeModules/FCLVersion.cmake b/CMakeModules/FCLVersion.cmake index 508259a91..0da9ffe38 100644 --- a/CMakeModules/FCLVersion.cmake +++ b/CMakeModules/FCLVersion.cmake @@ -1,8 +1,8 @@ # set the version in a way CMake can use set(FCL_MAJOR_VERSION 0) -set(FCL_MINOR_VERSION 4) +set(FCL_MINOR_VERSION 5) set(FCL_PATCH_VERSION 0) set(FCL_VERSION "${FCL_MAJOR_VERSION}.${FCL_MINOR_VERSION}.${FCL_PATCH_VERSION}") # increment this when we have ABI changes -set(FCL_ABI_VERSION 6) +set(FCL_ABI_VERSION 7) diff --git a/ci/install_linux.sh b/ci/install_linux.sh index 854587007..a6e54b3ab 100755 --- a/ci/install_linux.sh +++ b/ci/install_linux.sh @@ -1,18 +1,8 @@ sudo add-apt-repository --yes ppa:libccd-debs/ppa sudo apt-get -qq update -######################## -# Mendatory dependencies -######################## -sudo apt-get -qq --yes --force-yes install cmake -sudo apt-get -qq --yes --force-yes install libboost-all-dev sudo apt-get -qq --yes --force-yes install libccd-dev -######################## -# Optional dependencies -######################## -sudo apt-get -qq --yes --force-yes install libflann-dev - # Octomap git clone https://github.com/OctoMap/octomap cd octomap diff --git a/fcl.pc.in b/fcl.pc.in index 19182c344..a27fdfdff 100644 --- a/fcl.pc.in +++ b/fcl.pc.in @@ -9,4 +9,4 @@ Description: @PKG_DESC@ Version: @FCL_VERSION@ Requires: @PKG_EXTERNAL_DEPS@ Libs: -L${libdir} -lfcl -Cflags: -I${includedir} +Cflags: @PKG_CFLAGS@ -I${includedir} diff --git a/include/fcl/BV/RSS.h b/include/fcl/BV/RSS.h index 91fc442ae..9f19e9188 100644 --- a/include/fcl/BV/RSS.h +++ b/include/fcl/BV/RSS.h @@ -38,10 +38,10 @@ #ifndef FCL_RSS_H #define FCL_RSS_H - +#include "fcl/math/constants.h" #include "fcl/math/vec_3f.h" #include "fcl/math/matrix_3f.h" -#include + namespace fcl { @@ -111,7 +111,7 @@ class RSS /// @brief Volume of the RSS inline FCL_REAL volume() const { - return (l[0] * l[1] * 2 * r + 4 * boost::math::constants::pi() * r * r * r); + return (l[0] * l[1] * 2 * r + 4 * constants::pi * r * r * r); } /// @brief Size of the RSS (used in BV_Splitter to order two RSSs) diff --git a/include/fcl/BVH/BVH_model.h b/include/fcl/BVH/BVH_model.h index 69a30b3d1..e5a8c7cfb 100644 --- a/include/fcl/BVH/BVH_model.h +++ b/include/fcl/BVH/BVH_model.h @@ -44,7 +44,7 @@ #include "fcl/BVH/BV_splitter.h" #include "fcl/BVH/BV_fitter.h" #include -#include +#include #include namespace fcl @@ -268,10 +268,10 @@ class BVHModel : public CollisionGeometry, BVHBuildState build_state; /// @brief Split rule to split one BV node into two children - boost::shared_ptr > bv_splitter; + std::shared_ptr > bv_splitter; /// @brief Fitting rule to fit a BV node to a set of geometry primitives - boost::shared_ptr > bv_fitter; + std::shared_ptr > bv_fitter; private: diff --git a/include/fcl/articulated_model/joint.h b/include/fcl/articulated_model/joint.h index 1fa053465..4a87616dd 100644 --- a/include/fcl/articulated_model/joint.h +++ b/include/fcl/articulated_model/joint.h @@ -45,8 +45,7 @@ #include #include #include -#include -#include +#include namespace fcl { @@ -61,7 +60,7 @@ class Joint { public: - Joint(const boost::shared_ptr& link_parent, const boost::shared_ptr& link_child, + Joint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name); @@ -76,14 +75,14 @@ class Joint virtual std::size_t getNumDofs() const = 0; - boost::shared_ptr getJointConfig() const; - void setJointConfig(const boost::shared_ptr& joint_cfg); + std::shared_ptr getJointConfig() const; + void setJointConfig(const std::shared_ptr& joint_cfg); - boost::shared_ptr getParentLink() const; - boost::shared_ptr getChildLink() const; + std::shared_ptr getParentLink() const; + std::shared_ptr getChildLink() const; - void setParentLink(const boost::shared_ptr& link); - void setChildLink(const boost::shared_ptr& link); + void setParentLink(const std::shared_ptr& link); + void setChildLink(const std::shared_ptr& link); JointType getJointType() const; @@ -93,13 +92,13 @@ class Joint protected: /// links to parent and child are only for connection, so weak_ptr to avoid cyclic dependency - boost::weak_ptr link_parent_, link_child_; + std::weak_ptr link_parent_, link_child_; JointType type_; std::string name_; - boost::shared_ptr joint_cfg_; + std::shared_ptr joint_cfg_; Transform3f transform_to_parent_; }; @@ -108,7 +107,7 @@ class Joint class PrismaticJoint : public Joint { public: - PrismaticJoint(const boost::shared_ptr& link_parent, const boost::shared_ptr& link_child, + PrismaticJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name, const Vec3f& axis); @@ -128,7 +127,7 @@ class PrismaticJoint : public Joint class RevoluteJoint : public Joint { public: - RevoluteJoint(const boost::shared_ptr& link_parent, const boost::shared_ptr& link_child, + RevoluteJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name, const Vec3f& axis); @@ -150,7 +149,7 @@ class RevoluteJoint : public Joint class BallEulerJoint : public Joint { public: - BallEulerJoint(const boost::shared_ptr& link_parent, const boost::shared_ptr& link_child, + BallEulerJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name); diff --git a/include/fcl/articulated_model/joint_config.h b/include/fcl/articulated_model/joint_config.h index f87533d68..7229b7083 100644 --- a/include/fcl/articulated_model/joint_config.h +++ b/include/fcl/articulated_model/joint_config.h @@ -39,8 +39,7 @@ #define FCL_ARTICULATED_MODEL_JOINT_CONFIG_H #include "fcl/data_types.h" -#include -#include +#include #include namespace fcl @@ -55,7 +54,7 @@ class JointConfig JointConfig(const JointConfig& joint_cfg); - JointConfig(const boost::shared_ptr& joint, + JointConfig(const std::shared_ptr& joint, FCL_REAL default_value = 0, FCL_REAL default_value_min = 0, FCL_REAL default_value_max = 0); @@ -84,10 +83,10 @@ class JointConfig FCL_REAL& getLimitMax(std::size_t i); - boost::shared_ptr getJoint() const; + std::shared_ptr getJoint() const; private: - boost::weak_ptr joint_; + std::weak_ptr joint_; std::vector values_; std::vector limits_min_; diff --git a/include/fcl/articulated_model/link.h b/include/fcl/articulated_model/link.h index 1df1c4cb5..3e7ce262c 100644 --- a/include/fcl/articulated_model/link.h +++ b/include/fcl/articulated_model/link.h @@ -41,7 +41,7 @@ #include "fcl/data_types.h" #include "fcl/collision_object.h" -#include +#include #include namespace fcl @@ -58,11 +58,11 @@ class Link void setName(const std::string& name); - void addChildJoint(const boost::shared_ptr& joint); + void addChildJoint(const std::shared_ptr& joint); - void setParentJoint(const boost::shared_ptr& joint); + void setParentJoint(const std::shared_ptr& joint); - void addObject(const boost::shared_ptr& object); + void addObject(const std::shared_ptr& object); std::size_t getNumChildJoints() const; @@ -71,11 +71,11 @@ class Link protected: std::string name_; - std::vector > objects_; + std::vector > objects_; - std::vector > children_joints_; + std::vector > children_joints_; - boost::shared_ptr parent_joint_; + std::shared_ptr parent_joint_; }; } diff --git a/include/fcl/articulated_model/model.h b/include/fcl/articulated_model/model.h index bc95504cb..cfdfe0a5b 100644 --- a/include/fcl/articulated_model/model.h +++ b/include/fcl/articulated_model/model.h @@ -42,7 +42,7 @@ #include "fcl/articulated_model/link.h" #include "fcl/data_types.h" -#include +#include #include #include @@ -64,9 +64,9 @@ class Model const std::string& getName() const; - void addLink(const boost::shared_ptr& link); + void addLink(const std::shared_ptr& link); - void addJoint(const boost::shared_ptr& joint); + void addJoint(const std::shared_ptr& joint); void initRoot(const std::map& link_parent_tree); @@ -78,16 +78,16 @@ class Model std::size_t getNumJoints() const; - boost::shared_ptr getRoot() const; - boost::shared_ptr getLink(const std::string& name) const; - boost::shared_ptr getJoint(const std::string& name) const; + std::shared_ptr getRoot() const; + std::shared_ptr getLink(const std::string& name) const; + std::shared_ptr getJoint(const std::string& name) const; - std::vector > getLinks() const; - std::vector > getJoints() const; + std::vector > getLinks() const; + std::vector > getJoints() const; protected: - boost::shared_ptr root_link_; - std::map > links_; - std::map > joints_; + std::shared_ptr root_link_; + std::map > links_; + std::map > joints_; std::string name_; diff --git a/include/fcl/articulated_model/model_config.h b/include/fcl/articulated_model/model_config.h index 49e8a6405..ce6e59088 100644 --- a/include/fcl/articulated_model/model_config.h +++ b/include/fcl/articulated_model/model_config.h @@ -42,8 +42,7 @@ #include "fcl/articulated_model/joint_config.h" #include #include -#include -#include +#include namespace fcl { @@ -55,13 +54,13 @@ class ModelConfig ModelConfig(const ModelConfig& model_cfg); - ModelConfig(std::map > joints_map); + ModelConfig(std::map > joints_map); JointConfig getJointConfigByJointName(const std::string& joint_name) const; JointConfig& getJointConfigByJointName(const std::string& joint_name); - JointConfig getJointConfigByJoint(boost::shared_ptr joint) const; - JointConfig& getJointConfigByJoint(boost::shared_ptr joint); + JointConfig getJointConfigByJoint(std::shared_ptr joint) const; + JointConfig& getJointConfigByJoint(std::shared_ptr joint); std::map getJointCfgsMap() const { return joint_cfgs_map_; } diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h index 1b8ea8968..3509db9a5 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h @@ -43,8 +43,8 @@ #include "fcl/broadphase/hierarchy_tree.h" #include "fcl/BV/BV.h" #include "fcl/shape/geometric_shapes_utility.h" -#include -#include +#include +#include #include @@ -55,7 +55,7 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager { public: typedef NodeBase DynamicAABBNode; - typedef boost::unordered_map DynamicAABBTable; + typedef std::unordered_map DynamicAABBTable; int max_tree_nonbalanced_level; int tree_incremental_balance_pass; @@ -114,7 +114,7 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager void getObjects(std::vector& objs) const { objs.resize(this->size()); - std::transform(table.begin(), table.end(), objs.begin(), boost::bind(&DynamicAABBTable::value_type::first, _1)); + std::transform(table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } /// @brief perform collision test between one object and all the objects belonging to the manager @@ -152,7 +152,7 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager private: HierarchyTree dtree; - boost::unordered_map table; + std::unordered_map table; bool setup_; diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h index 3c132e092..0080be615 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -42,8 +42,8 @@ #include "fcl/broadphase/hierarchy_tree.h" #include "fcl/BV/BV.h" #include "fcl/shape/geometric_shapes_utility.h" -#include -#include +#include +#include #include @@ -54,7 +54,7 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager { public: typedef implementation_array::NodeBase DynamicAABBNode; - typedef boost::unordered_map DynamicAABBTable; + typedef std::unordered_map DynamicAABBTable; int max_tree_nonbalanced_level; int tree_incremental_balance_pass; @@ -112,7 +112,7 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager void getObjects(std::vector& objs) const { objs.resize(this->size()); - std::transform(table.begin(), table.end(), objs.begin(), boost::bind(&DynamicAABBTable::value_type::first, _1)); + std::transform(table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); } /// @brief perform collision test between one object and all the objects belonging to the manager @@ -150,7 +150,7 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager private: implementation_array::HierarchyTree dtree; - boost::unordered_map table; + std::unordered_map table; bool setup_; diff --git a/include/fcl/broadphase/broadphase_interval_tree.h b/include/fcl/broadphase/broadphase_interval_tree.h index eb9c7cd5b..6c68ae97f 100644 --- a/include/fcl/broadphase/broadphase_interval_tree.h +++ b/include/fcl/broadphase/broadphase_interval_tree.h @@ -109,9 +109,10 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager /// @brief the number of objects managed by the manager inline size_t size() const { return endpoints[0].size() / 2; } -protected: +protected: + /// @brief SAP end point struct EndPoint { @@ -123,6 +124,11 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager /// @brief tag for whether it is a lower bound or higher bound of an interval, 0 for lo, and 1 for hi char minmax; + + bool operator<(const EndPoint &p) const + { + return value < p.value; + } }; /// @brief Extention interval tree's interval to SAP interval, adding more information diff --git a/include/fcl/broadphase/hash.h b/include/fcl/broadphase/hash.h index 58686dbd7..84535826e 100644 --- a/include/fcl/broadphase/hash.h +++ b/include/fcl/broadphase/hash.h @@ -42,7 +42,7 @@ #include #include #include -#include +#include namespace fcl { @@ -123,7 +123,7 @@ class SimpleHashTable template -class unordered_map_hash_table : public boost::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> diff --git a/include/fcl/broadphase/hierarchy_tree.h b/include/fcl/broadphase/hierarchy_tree.h index a7130305a..d1e9fd488 100644 --- a/include/fcl/broadphase/hierarchy_tree.h +++ b/include/fcl/broadphase/hierarchy_tree.h @@ -40,9 +40,9 @@ #include #include +#include #include "fcl/BV/AABB.h" #include "fcl/broadphase/morton.h" -#include namespace fcl { diff --git a/include/fcl/broadphase/hierarchy_tree.hxx b/include/fcl/broadphase/hierarchy_tree.hxx index 10ce2f726..204556962 100644 --- a/include/fcl/broadphase/hierarchy_tree.hxx +++ b/include/fcl/broadphase/hierarchy_tree.hxx @@ -369,7 +369,7 @@ typename HierarchyTree::NodeType* HierarchyTree::topdown_0(const NodeVec // compute median NodeVecIterator lcenter = lbeg + num_leaves / 2; - std::nth_element(lbeg, lcenter, lend, boost::bind(&nodeBaseLess, _1, _2, boost::ref(best_axis))); + std::nth_element(lbeg, lcenter, lend, std::bind(&nodeBaseLess, std::placeholders::_1, std::placeholders::_2, std::ref(best_axis))); NodeType* node = createNode(NULL, vol, NULL); node->children[0] = topdown_0(lbeg, lcenter); diff --git a/include/fcl/ccd/interpolation/interpolation_factory.h b/include/fcl/ccd/interpolation/interpolation_factory.h index c8b3ec552..ac5fcf2c1 100644 --- a/include/fcl/ccd/interpolation/interpolation_factory.h +++ b/include/fcl/ccd/interpolation/interpolation_factory.h @@ -43,7 +43,7 @@ #include -#include +#include #include namespace fcl @@ -52,12 +52,12 @@ namespace fcl class InterpolationFactory { public: - typedef boost::function(FCL_REAL, FCL_REAL)> CreateFunction; + typedef boost::function(FCL_REAL, FCL_REAL)> CreateFunction; public: void registerClass(const InterpolationType type, const CreateFunction create_function); - boost::shared_ptr create(const InterpolationType type, FCL_REAL start_value, FCL_REAL end_value); + std::shared_ptr create(const InterpolationType type, FCL_REAL start_value, FCL_REAL end_value); public: static InterpolationFactory& instance(); diff --git a/include/fcl/ccd/interpolation/interpolation_linear.h b/include/fcl/ccd/interpolation/interpolation_linear.h index ecef37344..261b1b0ad 100644 --- a/include/fcl/ccd/interpolation/interpolation_linear.h +++ b/include/fcl/ccd/interpolation/interpolation_linear.h @@ -41,7 +41,7 @@ #include "fcl/data_types.h" #include "fcl/ccd/interpolation/interpolation.h" -#include +#include namespace fcl { @@ -67,7 +67,7 @@ class InterpolationLinear : public Interpolation virtual FCL_REAL getVelocityBound(FCL_REAL time) const; public: - static boost::shared_ptr create(FCL_REAL start_value, FCL_REAL end_value); + static std::shared_ptr create(FCL_REAL start_value, FCL_REAL end_value); static void registerToFactory(); }; diff --git a/include/fcl/ccd/motion_base.h b/include/fcl/ccd/motion_base.h index b675a3bd0..bffd1bd54 100644 --- a/include/fcl/ccd/motion_base.h +++ b/include/fcl/ccd/motion_base.h @@ -117,7 +117,7 @@ class TriangleMotionBoundVisitor class MotionBase { public: - MotionBase() : time_interval_(boost::shared_ptr(new TimeInterval(0, 1))) + MotionBase() : time_interval_(std::shared_ptr(new TimeInterval(0, 1))) { } @@ -174,17 +174,17 @@ class MotionBase virtual void getTaylorModel(TMatrix3& tm, TVector3& tv) const = 0; - const boost::shared_ptr& getTimeInterval() const + const std::shared_ptr& getTimeInterval() const { return time_interval_; } protected: - boost::shared_ptr time_interval_; + std::shared_ptr time_interval_; }; -typedef boost::shared_ptr MotionBasePtr; +typedef std::shared_ptr MotionBasePtr; } diff --git a/include/fcl/ccd/taylor_matrix.h b/include/fcl/ccd/taylor_matrix.h index a27435b79..2f0d724af 100644 --- a/include/fcl/ccd/taylor_matrix.h +++ b/include/fcl/ccd/taylor_matrix.h @@ -52,10 +52,10 @@ class TMatrix3 public: TMatrix3(); - TMatrix3(const boost::shared_ptr& time_interval); + TMatrix3(const std::shared_ptr& time_interval); TMatrix3(TaylorModel m[3][3]); TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3); - TMatrix3(const Matrix3f& m, const boost::shared_ptr& time_interval); + TMatrix3(const Matrix3f& m, const std::shared_ptr& time_interval); TVector3 getColumn(size_t i) const; const TVector3& getRow(size_t i) const; @@ -99,10 +99,10 @@ class TMatrix3 void setZero(); FCL_REAL diameter() const; - void setTimeInterval(const boost::shared_ptr& time_interval); + void setTimeInterval(const std::shared_ptr& time_interval); void setTimeInterval(FCL_REAL l, FCL_REAL r); - const boost::shared_ptr& getTimeInterval() const; + const std::shared_ptr& getTimeInterval() const; TMatrix3& rotationConstrain(); }; diff --git a/include/fcl/ccd/taylor_model.h b/include/fcl/ccd/taylor_model.h index b53b01406..8c790bc18 100644 --- a/include/fcl/ccd/taylor_model.h +++ b/include/fcl/ccd/taylor_model.h @@ -39,7 +39,7 @@ #define FCL_CCD_TAYLOR_MODEL_H #include "fcl/ccd/interval.h" -#include +#include namespace fcl { @@ -77,7 +77,7 @@ struct TimeInterval class TaylorModel { /// @brief time interval - boost::shared_ptr time_interval_; + std::shared_ptr time_interval_; /// @brief Coefficients of the cubic polynomial approximation FCL_REAL coeffs_[4]; @@ -92,12 +92,12 @@ class TaylorModel time_interval_->setValue(l, r); } - void setTimeInterval(const boost::shared_ptr& time_interval) + void setTimeInterval(const std::shared_ptr& time_interval) { time_interval_ = time_interval; } - const boost::shared_ptr& getTimeInterval() const + const std::shared_ptr& getTimeInterval() const { return time_interval_; } @@ -109,10 +109,10 @@ class TaylorModel TaylorModel(); - TaylorModel(const boost::shared_ptr& time_interval); - TaylorModel(FCL_REAL coeff, const boost::shared_ptr& time_interval); - TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr& time_interval); - TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr& time_interval); + TaylorModel(const std::shared_ptr& time_interval); + TaylorModel(FCL_REAL coeff, const std::shared_ptr& time_interval); + TaylorModel(FCL_REAL coeffs[3], const Interval& r, const std::shared_ptr& time_interval); + TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const std::shared_ptr& time_interval); TaylorModel operator + (const TaylorModel& other) const; TaylorModel& operator += (const TaylorModel& other); diff --git a/include/fcl/ccd/taylor_vector.h b/include/fcl/ccd/taylor_vector.h index 52a68db2b..0f6067ec8 100644 --- a/include/fcl/ccd/taylor_vector.h +++ b/include/fcl/ccd/taylor_vector.h @@ -51,10 +51,10 @@ class TVector3 public: TVector3(); - TVector3(const boost::shared_ptr& time_interval); + TVector3(const std::shared_ptr& time_interval); TVector3(TaylorModel v[3]); TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2); - TVector3(const Vec3f& v, const boost::shared_ptr& time_interval); + TVector3(const Vec3f& v, const std::shared_ptr& time_interval); TVector3 operator + (const TVector3& other) const; TVector3& operator += (const TVector3& other); @@ -96,10 +96,10 @@ class TVector3 TaylorModel squareLength() const; - void setTimeInterval(const boost::shared_ptr& time_interval); + void setTimeInterval(const std::shared_ptr& time_interval); void setTimeInterval(FCL_REAL l, FCL_REAL r); - const boost::shared_ptr& getTimeInterval() const; + const std::shared_ptr& getTimeInterval() const; }; void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity); diff --git a/include/fcl/collision_object.h b/include/fcl/collision_object.h index 0e41fbb16..39e15197e 100644 --- a/include/fcl/collision_object.h +++ b/include/fcl/collision_object.h @@ -43,7 +43,7 @@ #include "fcl/BV/AABB.h" #include "fcl/math/transform.h" #include "fcl/ccd/motion_base.h" -#include +#include namespace fcl { @@ -151,7 +151,7 @@ class CollisionGeometry class CollisionObject { public: - CollisionObject(const boost::shared_ptr &cgeom_) : + CollisionObject(const std::shared_ptr &cgeom_) : cgeom(cgeom_), cgeom_const(cgeom_) { if (cgeom) @@ -161,14 +161,14 @@ class CollisionObject } } - CollisionObject(const boost::shared_ptr &cgeom_, const Transform3f& tf) : + CollisionObject(const std::shared_ptr &cgeom_, const Transform3f& tf) : cgeom(cgeom_), cgeom_const(cgeom_), t(tf) { cgeom->computeLocalAABB(); computeAABB(); } - CollisionObject(const boost::shared_ptr &cgeom_, const Matrix3f& R, const Vec3f& T): + CollisionObject(const std::shared_ptr &cgeom_, const Matrix3f& R, const Vec3f& T): cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3f(R, T)) { cgeom->computeLocalAABB(); @@ -305,7 +305,7 @@ class CollisionObject } /// @brief get geometry from the object instance - const boost::shared_ptr& collisionGeometry() const + const std::shared_ptr& collisionGeometry() const { return cgeom_const; } @@ -342,8 +342,8 @@ class CollisionObject protected: - boost::shared_ptr cgeom; - boost::shared_ptr cgeom_const; + std::shared_ptr cgeom; + std::shared_ptr cgeom_const; Transform3f t; @@ -359,12 +359,12 @@ class CollisionObject class ContinuousCollisionObject { public: - ContinuousCollisionObject(const boost::shared_ptr& cgeom_) : + ContinuousCollisionObject(const std::shared_ptr& cgeom_) : cgeom(cgeom_), cgeom_const(cgeom_) { } - ContinuousCollisionObject(const boost::shared_ptr& cgeom_, const boost::shared_ptr& motion_) : + ContinuousCollisionObject(const std::shared_ptr& cgeom_, const std::shared_ptr& motion_) : cgeom(cgeom_), cgeom_const(cgeom), motion(motion_) { } @@ -455,17 +455,17 @@ class ContinuousCollisionObject } /// @brief get geometry from the object instance - inline const boost::shared_ptr& collisionGeometry() const + inline const std::shared_ptr& collisionGeometry() const { return cgeom_const; } protected: - boost::shared_ptr cgeom; - boost::shared_ptr cgeom_const; + std::shared_ptr cgeom; + std::shared_ptr cgeom_const; - boost::shared_ptr motion; + std::shared_ptr motion; /// @brief AABB in the global coordinate for the motion mutable AABB aabb; diff --git a/include/fcl/intersect.h b/include/fcl/intersect.h index ac5153f82..f7703d417 100644 --- a/include/fcl/intersect.h +++ b/include/fcl/intersect.h @@ -237,7 +237,7 @@ class Intersect /// @brief compute the cdf(x) static FCL_REAL gaussianCDF(FCL_REAL x) { - return 0.5 * boost::math::erfc(-x / sqrt(2.0)); + return 0.5 * std::erfc(-x / sqrt(2.0)); } diff --git a/include/fcl/knn/nearest_neighbors.h b/include/fcl/knn/nearest_neighbors.h index fccd9daef..a789f280c 100644 --- a/include/fcl/knn/nearest_neighbors.h +++ b/include/fcl/knn/nearest_neighbors.h @@ -39,7 +39,7 @@ #define FCL_KNN_NEAREST_NEIGHBORS_H #include -#include +#include #include namespace fcl diff --git a/include/fcl/knn/nearest_neighbors_flann.h b/include/fcl/knn/nearest_neighbors_flann.h index c63ff4c8b..66f0a99e3 100644 --- a/include/fcl/knn/nearest_neighbors_flann.h +++ b/include/fcl/knn/nearest_neighbors_flann.h @@ -87,7 +87,7 @@ class NearestNeighborsFLANN : public NearestNeighbors<_T> { public: - NearestNeighborsFLANN(const boost::shared_ptr ¶ms) + NearestNeighborsFLANN(const std::shared_ptr ¶ms) : index_(0), params_(params), searchParams_(32, 0., true), dimension_(1) { } @@ -228,14 +228,14 @@ class NearestNeighborsFLANN : public NearestNeighbors<_T> /// /// The parameters determine the type of nearest neighbor /// data structure to be constructed. - virtual void setIndexParams(const boost::shared_ptr ¶ms) + virtual void setIndexParams(const std::shared_ptr ¶ms) { params_ = params; rebuildIndex(); } /// \brief Get the FLANN parameters used to build the current index. - virtual const boost::shared_ptr& getIndexParams(void) const + virtual const std::shared_ptr& getIndexParams(void) const { return params_; } @@ -300,7 +300,7 @@ class NearestNeighborsFLANN : public NearestNeighbors<_T> /// \brief The FLANN index parameters. This contains both the type of /// index and the parameters for that type. - boost::shared_ptr params_; + std::shared_ptr params_; /// \brief The parameters used to seach for nearest neighbors. mutable flann::SearchParams searchParams_; @@ -325,7 +325,7 @@ class NearestNeighborsFLANNLinear : public NearestNeighborsFLANN<_T, _Dist> public: NearestNeighborsFLANNLinear() : NearestNeighborsFLANN<_T, _Dist>( - boost::shared_ptr( + std::shared_ptr( new flann::LinearIndexParams())) { } @@ -337,7 +337,7 @@ class NearestNeighborsFLANNHierarchicalClustering : public NearestNeighborsFLANN public: NearestNeighborsFLANNHierarchicalClustering() : NearestNeighborsFLANN<_T, _Dist>( - boost::shared_ptr( + std::shared_ptr( new flann::HierarchicalClusteringIndexParams())) { } diff --git a/include/fcl/math/constants.h b/include/fcl/math/constants.h new file mode 100644 index 000000000..a5be83772 --- /dev/null +++ b/include/fcl/math/constants.h @@ -0,0 +1,54 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Rice University + * 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 Rice University 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 Mark Moll */ + +#ifndef FCL_MATH_CONSTANTS_ +#define FCL_MATH_CONSTANTS_ + +#include "fcl/data_types.h" + +namespace fcl +{ + namespace constants + { + /// The mathematical constant pi + constexpr FCL_REAL pi = FCL_REAL(3.141592653589793238462643383279502884197169399375105820974944592L); + + /// The golden ratio + constexpr FCL_REAL phi = FCL_REAL(1.618033988749894848204586834365638117720309179805762862135448623L); + } +} + +#endif diff --git a/include/fcl/math/sampling.h b/include/fcl/math/sampling.h index 5ce227674..3c2a9faed 100644 --- a/include/fcl/math/sampling.h +++ b/include/fcl/math/sampling.h @@ -5,8 +5,8 @@ #include #include #include -#include #include +#include "fcl/math/constants.h" #include "fcl/math/vec_nf.h" #include "fcl/math/transform.h" @@ -186,8 +186,7 @@ class SamplerSE2 : public SamplerBase Vecnf<3> q; q[0] = rng.uniformReal(lower_bound[0], lower_bound[1]); q[1] = rng.uniformReal(lower_bound[1], lower_bound[2]); - q[2] = rng.uniformReal(-boost::math::constants::pi(), - boost::math::constants::pi()); + q[2] = rng.uniformReal(-constants::pi, constants::pi); return q; } @@ -227,8 +226,7 @@ class SamplerSE2_disk : public SamplerBase rng.disk(r_min, r_max, x, y); q[0] = x + c[0] - cref[0]; q[1] = y + c[1] - cref[1]; - q[2] = rng.uniformReal(-boost::math::constants::pi(), - boost::math::constants::pi()); + q[2] = rng.uniformReal(-constants::pi, constants::pi); return q; } diff --git a/include/fcl/octree.h b/include/fcl/octree.h index 451224e35..7b3c6815d 100644 --- a/include/fcl/octree.h +++ b/include/fcl/octree.h @@ -40,7 +40,7 @@ #define FCL_OCTREE_H -#include +#include #include #include @@ -54,7 +54,7 @@ namespace fcl class OcTree : public CollisionGeometry { private: - boost::shared_ptr tree; + std::shared_ptr tree; FCL_REAL default_occupancy; @@ -70,7 +70,7 @@ class OcTree : public CollisionGeometry typedef octomap::OcTreeNode OcTreeNode; /// @brief construct octree with a given resolution - OcTree(FCL_REAL resolution) : tree(boost::shared_ptr(new octomap::OcTree(resolution))) + OcTree(FCL_REAL resolution) : tree(std::shared_ptr(new octomap::OcTree(resolution))) { default_occupancy = tree->getOccupancyThres(); @@ -80,7 +80,7 @@ class OcTree : public CollisionGeometry } /// @brief construct octree from octomap - OcTree(const boost::shared_ptr& tree_) : tree(tree_) + OcTree(const std::shared_ptr& tree_) : tree(tree_) { default_occupancy = tree->getOccupancyThres(); diff --git a/include/fcl/profile.h b/include/fcl/profile.h index 74add5bf0..9001fc94e 100644 --- a/include/fcl/profile.h +++ b/include/fcl/profile.h @@ -58,7 +58,8 @@ #include #include #include -#include +#include +#include #include #include @@ -332,8 +333,8 @@ class Profiler : private boost::noncopyable void printThreadInfo(std::ostream &out, const PerThread &data); - boost::mutex lock_; - std::map data_; + std::mutex lock_; + std::map data_; TimeInfo tinfo_; bool running_; bool printOnDestroy_; diff --git a/include/fcl/shape/geometric_shape_to_BVH_model.h b/include/fcl/shape/geometric_shape_to_BVH_model.h index d729707bd..c26a66115 100644 --- a/include/fcl/shape/geometric_shape_to_BVH_model.h +++ b/include/fcl/shape/geometric_shape_to_BVH_model.h @@ -97,7 +97,7 @@ void generateBVHModel(BVHModel& model, const Sphere& shape, const Transform3 double r = shape.radius; double phi, phid; - const double pi = boost::math::constants::pi(); + const double pi = constants::pi; phid = pi * 2 / seg; phi = 0; @@ -179,7 +179,7 @@ void generateBVHModel(BVHModel& model, const Ellipsoid& shape, const Transfo const FCL_REAL& c = shape.radii[2]; FCL_REAL phi, phid; - const FCL_REAL pi = boost::math::constants::pi(); + const FCL_REAL pi = constants::pi; phid = pi * 2 / seg; phi = 0; @@ -267,7 +267,7 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transfor double r = shape.radius; double h = shape.lz; double phi, phid; - const double pi = boost::math::constants::pi(); + const double pi = constants::pi; phid = pi * 2 / tot; phi = 0; @@ -338,7 +338,7 @@ void generateBVHModel(BVHModel& model, const Cylinder& shape, const Transfor double r = shape.radius; double h = shape.lz; - const double pi = boost::math::constants::pi(); + const double pi = constants::pi; unsigned int tot = tot_for_unit_cylinder * r; double phid = pi * 2 / tot; @@ -360,7 +360,7 @@ void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& double h = shape.lz; double phi, phid; - const double pi = boost::math::constants::pi(); + const double pi = constants::pi; phid = pi * 2 / tot; phi = 0; @@ -430,7 +430,7 @@ void generateBVHModel(BVHModel& model, const Cone& shape, const Transform3f& double r = shape.radius; double h = shape.lz; - const double pi = boost::math::constants::pi(); + const double pi = constants::pi; unsigned int tot = tot_for_unit_cone * r; double phid = pi * 2 / tot; diff --git a/include/fcl/shape/geometric_shapes.h b/include/fcl/shape/geometric_shapes.h index 6ac9146cb..578ec3cc6 100644 --- a/include/fcl/shape/geometric_shapes.h +++ b/include/fcl/shape/geometric_shapes.h @@ -139,7 +139,7 @@ class Sphere : public ShapeBase FCL_REAL computeVolume() const { - return 4.0 * boost::math::constants::pi() * radius * radius * radius / 3.0; + return 4.0 * constants::pi * radius * radius * radius / 3.0; } }; @@ -177,7 +177,7 @@ class Ellipsoid : public ShapeBase FCL_REAL computeVolume() const { - const FCL_REAL pi = boost::math::constants::pi(); + const FCL_REAL pi = constants::pi; return 4.0 * pi * radii[0] * radii[1] * radii[2] / 3.0; } }; @@ -204,13 +204,13 @@ class Capsule : public ShapeBase FCL_REAL computeVolume() const { - return boost::math::constants::pi() * radius * radius *(lz + radius * 4/3.0); + return constants::pi * radius * radius *(lz + radius * 4/3.0); } Matrix3f computeMomentofInertia() const { - FCL_REAL v_cyl = radius * radius * lz * boost::math::constants::pi(); - FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi() * 4 / 3.0; + FCL_REAL v_cyl = radius * radius * lz * constants::pi; + FCL_REAL v_sph = radius * radius * radius * constants::pi * 4 / 3.0; FCL_REAL ix = v_cyl * lz * lz / 12.0 + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + 0.25 * v_sph * lz * lz; FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius; @@ -244,7 +244,7 @@ class Cone : public ShapeBase FCL_REAL computeVolume() const { - return boost::math::constants::pi() * radius * radius * lz / 3; + return constants::pi * radius * radius * lz / 3; } Matrix3f computeMomentofInertia() const @@ -287,7 +287,7 @@ class Cylinder : public ShapeBase FCL_REAL computeVolume() const { - return boost::math::constants::pi() * radius * radius * lz; + return constants::pi * radius * radius * lz; } Matrix3f computeMomentofInertia() const diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h index ac0508243..544cfb130 100644 --- a/include/fcl/traversal/traversal_node_bvhs.h +++ b/include/fcl/traversal/traversal_node_bvhs.h @@ -48,7 +48,7 @@ #include "fcl/ccd/motion.h" #include -#include +#include #include #include #include diff --git a/include/fcl/traversal/traversal_node_octree.h b/include/fcl/traversal/traversal_node_octree.h index 24e056bd3..464554caf 100644 --- a/include/fcl/traversal/traversal_node_octree.h +++ b/include/fcl/traversal/traversal_node_octree.h @@ -347,7 +347,7 @@ class OcTreeSolver // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. if (free_space < contacts.size()) { - std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), boost::bind(comparePenDepth, _2, _1)); + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); num_adding_contacts = free_space; } else @@ -919,7 +919,7 @@ class OcTreeSolver // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. if (free_space < contacts.size()) { - std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), boost::bind(comparePenDepth, _2, _1)); + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); num_adding_contacts = free_space; } else diff --git a/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h index 809b581b5..0007d8710 100644 --- a/include/fcl/traversal/traversal_node_shapes.h +++ b/include/fcl/traversal/traversal_node_shapes.h @@ -91,7 +91,7 @@ class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase // If the free space is not enough to add all the new contacts, we add contacts in descent order of penetration depth. if (free_space < contacts.size()) { - std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), boost::bind(comparePenDepth, _2, _1)); + std::partial_sort(contacts.begin(), contacts.begin() + free_space, contacts.end(), std::bind(comparePenDepth, std::placeholders::_2, std::placeholders::_1)); num_adding_contacts = free_space; } else diff --git a/src/articulated_model/joint.cpp b/src/articulated_model/joint.cpp index 645312307..6b37c7d30 100644 --- a/src/articulated_model/joint.cpp +++ b/src/articulated_model/joint.cpp @@ -42,7 +42,7 @@ namespace fcl { -Joint::Joint(const boost::shared_ptr& link_parent, const boost::shared_ptr& link_child, +Joint::Joint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name) : link_parent_(link_parent), link_child_(link_child), @@ -65,32 +65,32 @@ void Joint::setName(const std::string& name) name_ = name; } -boost::shared_ptr Joint::getJointConfig() const +std::shared_ptr Joint::getJointConfig() const { return joint_cfg_; } -void Joint::setJointConfig(const boost::shared_ptr& joint_cfg) +void Joint::setJointConfig(const std::shared_ptr& joint_cfg) { joint_cfg_ = joint_cfg; } -boost::shared_ptr Joint::getParentLink() const +std::shared_ptr Joint::getParentLink() const { return link_parent_.lock(); } -boost::shared_ptr Joint::getChildLink() const +std::shared_ptr Joint::getChildLink() const { return link_child_.lock(); } -void Joint::setParentLink(const boost::shared_ptr& link) +void Joint::setParentLink(const std::shared_ptr& link) { link_parent_ = link; } -void Joint::setChildLink(const boost::shared_ptr& link) +void Joint::setChildLink(const std::shared_ptr& link) { link_child_ = link; } @@ -111,7 +111,7 @@ void Joint::setTransformToParent(const Transform3f& t) } -PrismaticJoint::PrismaticJoint(const boost::shared_ptr& link_parent, const boost::shared_ptr& link_child, +PrismaticJoint::PrismaticJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name, const Vec3f& axis) : @@ -139,7 +139,7 @@ Transform3f PrismaticJoint::getLocalTransform() const } -RevoluteJoint::RevoluteJoint(const boost::shared_ptr& link_parent, const boost::shared_ptr& link_child, +RevoluteJoint::RevoluteJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name, const Vec3f& axis) : @@ -167,7 +167,7 @@ Transform3f RevoluteJoint::getLocalTransform() const } -BallEulerJoint::BallEulerJoint(const boost::shared_ptr& link_parent, const boost::shared_ptr& link_child, +BallEulerJoint::BallEulerJoint(const std::shared_ptr& link_parent, const std::shared_ptr& link_child, const Transform3f& transform_to_parent, const std::string& name) : Joint(link_parent, link_child, transform_to_parent, name) diff --git a/src/articulated_model/joint_config.cpp b/src/articulated_model/joint_config.cpp index 9a65ff65d..f718330dd 100644 --- a/src/articulated_model/joint_config.cpp +++ b/src/articulated_model/joint_config.cpp @@ -51,7 +51,7 @@ JointConfig::JointConfig(const JointConfig& joint_cfg) : { } -JointConfig::JointConfig(const boost::shared_ptr& joint, +JointConfig::JointConfig(const std::shared_ptr& joint, FCL_REAL default_value, FCL_REAL default_value_min, FCL_REAL default_value_max) : @@ -99,7 +99,7 @@ FCL_REAL& JointConfig::getLimitMax(std::size_t i) -boost::shared_ptr JointConfig::getJoint() const +std::shared_ptr JointConfig::getJoint() const { return joint_.lock(); } diff --git a/src/articulated_model/link.cpp b/src/articulated_model/link.cpp index c8c68df26..9afbb6b4e 100644 --- a/src/articulated_model/link.cpp +++ b/src/articulated_model/link.cpp @@ -55,17 +55,17 @@ void Link::setName(const std::string& name) name_ = name; } -void Link::addChildJoint(const boost::shared_ptr& joint) +void Link::addChildJoint(const std::shared_ptr& joint) { children_joints_.push_back(joint); } -void Link::setParentJoint(const boost::shared_ptr& joint) +void Link::setParentJoint(const std::shared_ptr& joint) { parent_joint_ = joint; } -void Link::addObject(const boost::shared_ptr& object) +void Link::addObject(const std::shared_ptr& object) { objects_.push_back(object); } diff --git a/src/articulated_model/model.cpp b/src/articulated_model/model.cpp index d95cc95c0..693c9dc3a 100644 --- a/src/articulated_model/model.cpp +++ b/src/articulated_model/model.cpp @@ -43,15 +43,15 @@ namespace fcl { -boost::shared_ptr Model::getRoot() const +std::shared_ptr Model::getRoot() const { return root_link_; } -boost::shared_ptr Model::getLink(const std::string& name) const +std::shared_ptr Model::getLink(const std::string& name) const { - boost::shared_ptr ptr; - std::map >::const_iterator it = links_.find(name); + std::shared_ptr ptr; + std::map >::const_iterator it = links_.find(name); if(it == links_.end()) ptr.reset(); else @@ -59,10 +59,10 @@ boost::shared_ptr Model::getLink(const std::string& name) const return ptr; } -boost::shared_ptr Model::getJoint(const std::string& name) const +std::shared_ptr Model::getJoint(const std::string& name) const { - boost::shared_ptr ptr; - std::map >::const_iterator it = joints_.find(name); + std::shared_ptr ptr; + std::map >::const_iterator it = joints_.find(name); if(it == joints_.end()) ptr.reset(); else @@ -75,10 +75,10 @@ const std::string& Model::getName() const return name_; } -std::vector > Model::getLinks() const +std::vector > Model::getLinks() const { - std::vector > links; - for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) + std::vector > links; + for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) { links.push_back(it->second); } @@ -99,7 +99,7 @@ std::size_t Model::getNumJoints() const std::size_t Model::getNumDofs() const { std::size_t dof = 0; - for(std::map >::const_iterator it = joints_.begin(); it != joints_.end(); ++it) + for(std::map >::const_iterator it = joints_.begin(); it != joints_.end(); ++it) { dof += it->second->getNumDofs(); } @@ -107,12 +107,12 @@ std::size_t Model::getNumDofs() const return dof; } -void Model::addLink(const boost::shared_ptr& link) +void Model::addLink(const std::shared_ptr& link) { links_[link->getName()] = link; } -void Model::addJoint(const boost::shared_ptr& joint) +void Model::addJoint(const std::shared_ptr& joint) { joints_[joint->getName()] = joint; } @@ -122,7 +122,7 @@ void Model::initRoot(const std::map& link_parent_tree) root_link_.reset(); /// find the links that have no parent in the tree - for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) + for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it) { std::map::const_iterator parent = link_parent_tree.find(it->first); if(parent == link_parent_tree.end()) @@ -144,7 +144,7 @@ void Model::initRoot(const std::map& link_parent_tree) void Model::initTree(std::map& link_parent_tree) { - for(std::map >::iterator it = joints_.begin(); it != joints_.end(); ++it) + for(std::map >::iterator it = joints_.begin(); it != joints_.end(); ++it) { std::string parent_link_name = it->second->getParentLink()->getName(); std::string child_link_name = it->second->getChildLink()->getName(); diff --git a/src/articulated_model/model_config.cpp b/src/articulated_model/model_config.cpp index 31af96288..514d6e7db 100644 --- a/src/articulated_model/model_config.cpp +++ b/src/articulated_model/model_config.cpp @@ -53,9 +53,9 @@ ModelConfig::ModelConfig(const ModelConfig& model_cfg) : joint_cfgs_map_(model_cfg.joint_cfgs_map_) {} -ModelConfig::ModelConfig(std::map > joints_map) +ModelConfig::ModelConfig(std::map > joints_map) { - std::map >::iterator it; + std::map >::iterator it; for(it = joints_map.begin(); it != joints_map.end(); ++it) joint_cfgs_map_[it->first] = JointConfig(it->second); } @@ -76,12 +76,12 @@ JointConfig& ModelConfig::getJointConfigByJointName(const std::string& joint_nam return it->second; } -JointConfig ModelConfig::getJointConfigByJoint(boost::shared_ptr joint) const +JointConfig ModelConfig::getJointConfigByJoint(std::shared_ptr joint) const { return getJointConfigByJointName(joint->getName()); } -JointConfig& ModelConfig::getJointConfigByJoint(boost::shared_ptr joint) +JointConfig& ModelConfig::getJointConfigByJoint(std::shared_ptr joint) { return getJointConfigByJointName(joint->getName()); } diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index bd420c377..72d847893 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -79,7 +79,7 @@ struct SortByZLow class DummyCollisionObject : public CollisionObject { public: - DummyCollisionObject(const AABB& aabb_) : CollisionObject(boost::shared_ptr()) + DummyCollisionObject(const AABB& aabb_) : CollisionObject(std::shared_ptr()) { aabb = aabb_; } diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index 34667b00a..3cd49a39e 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -38,7 +38,7 @@ #include "fcl/broadphase/broadphase_SaP.h" #include #include -#include +#include namespace fcl { @@ -120,9 +120,9 @@ void SaPCollisionManager::registerObjects(const std::vector& o for(size_t coord = 0; coord < 3; ++coord) { std::sort(endpoints.begin(), endpoints.end(), - boost::bind(std::less(), - boost::bind(static_cast(&EndPoint::getVal), _1, coord), - boost::bind(static_cast(&EndPoint::getVal), _2, coord))); + std::bind(std::less(), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, coord), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, coord))); endpoints[0]->prev[coord] = NULL; endpoints[0]->next[coord] = endpoints[1]; @@ -513,9 +513,9 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionC // compute stop_pos by binary search, this is cheaper than check it in while iteration linearly std::vector::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, - boost::bind(std::less(), - boost::bind(static_cast(&EndPoint::getVal), _1, axis), - boost::bind(static_cast(&EndPoint::getVal), _2, axis))); + std::bind(std::less(), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); EndPoint* end_pos = NULL; if(res_it != velist[axis].end()) @@ -579,9 +579,9 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC std::vector::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy, - boost::bind(std::less(), - boost::bind(static_cast(&EndPoint::getVal), _1, axis), - boost::bind(static_cast(&EndPoint::getVal), _2, axis))); + std::bind(std::less(), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_1, axis), + std::bind(static_cast(&EndPoint::getVal), std::placeholders::_2, axis))); EndPoint* end_pos = NULL; if(res_it != velist[axis].end()) diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 1cec6cb30..66bf37919 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -74,7 +74,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, box->cost_density = tree2->getDefaultOccupancy(); - CollisionObject obj2(boost::shared_ptr(box), box_tf); + CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } } @@ -108,7 +108,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, box->cost_density = root2->getOccupancy(); box->threshold_occupied = tree2->getOccupancyThres(); - CollisionObject obj2(boost::shared_ptr(box), box_tf); + CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } else return false; @@ -173,7 +173,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, box->cost_density = tree2->getOccupancyThres(); // thresholds are 0, 1, so uncertain - CollisionObject obj2(boost::shared_ptr(box), box_tf); + CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } } @@ -204,7 +204,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, box->cost_density = root2->getOccupancy(); box->threshold_occupied = tree2->getOccupancyThres(); - CollisionObject obj2(boost::shared_ptr(box), box_tf); + CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } else return false; @@ -257,7 +257,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(boost::shared_ptr(box), box_tf); + CollisionObject obj(std::shared_ptr(box), box_tf); return callback(static_cast(root1->data), &obj, cdata, min_dist); } else return false; @@ -346,7 +346,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(boost::shared_ptr(box), box_tf); + CollisionObject obj(std::shared_ptr(box), box_tf); return callback(static_cast(root1->data), &obj, cdata, min_dist); } else return false; diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 246d84d63..4ec168486 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -74,7 +74,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n box->cost_density = tree2->getDefaultOccupancy(); - CollisionObject obj2(boost::shared_ptr(box), box_tf); + CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } } @@ -107,7 +107,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n box->cost_density = root2->getOccupancy(); box->threshold_occupied = tree2->getOccupancyThres(); - CollisionObject obj2(boost::shared_ptr(box), box_tf); + CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } else return false; @@ -175,7 +175,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n box->cost_density = tree2->getDefaultOccupancy(); - CollisionObject obj2(boost::shared_ptr(box), box_tf); + CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } } @@ -205,7 +205,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n box->cost_density = root2->getOccupancy(); box->threshold_occupied = tree2->getOccupancyThres(); - CollisionObject obj2(boost::shared_ptr(box), box_tf); + CollisionObject obj2(std::shared_ptr(box), box_tf); return callback(obj1, &obj2, cdata); } else return false; @@ -260,7 +260,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* no Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(boost::shared_ptr(box), box_tf); + CollisionObject obj(std::shared_ptr(box), box_tf); return callback(static_cast(root1->data), &obj, cdata, min_dist); } else return false; @@ -342,7 +342,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* no Box* box = new Box(); Transform3f box_tf; constructBox(root2_bv, tf2, *box, box_tf); - CollisionObject obj(boost::shared_ptr(box), box_tf); + CollisionObject obj(std::shared_ptr(box), box_tf); return callback(static_cast(root1->data), &obj, cdata, min_dist); } else return false; diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index 92a365187..e9448591a 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -38,7 +38,8 @@ #include "fcl/broadphase/broadphase_interval_tree.h" #include #include -#include +#include + namespace fcl { @@ -50,9 +51,9 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) EndPoint p; p.value = obj->getAABB().min_[0]; - std::vector::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + std::vector::iterator start1 = std::lower_bound(endpoints[0].begin(), endpoints[0].end(), p); p.value = obj->getAABB().max_[0]; - std::vector::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + std::vector::iterator end1 = std::upper_bound(start1, endpoints[0].end(), p); if(start1 < end1) { @@ -76,9 +77,9 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) } p.value = obj->getAABB().min_[1]; - std::vector::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + std::vector::iterator start2 = std::lower_bound(endpoints[1].begin(), endpoints[1].end(), p); p.value = obj->getAABB().max_[1]; - std::vector::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + std::vector::iterator end2 = std::upper_bound(start2, endpoints[1].end(), p); if(start2 < end2) { @@ -103,9 +104,9 @@ void IntervalTreeCollisionManager::unregisterObject(CollisionObject* obj) p.value = obj->getAABB().min_[2]; - std::vector::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + std::vector::iterator start3 = std::lower_bound(endpoints[2].begin(), endpoints[2].end(), p); p.value = obj->getAABB().max_[2]; - std::vector::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + std::vector::iterator end3 = std::upper_bound(start3, endpoints[2].end(), p); if(start3 < end3) { @@ -178,9 +179,9 @@ void IntervalTreeCollisionManager::setup() { if(!setup_) { - std::sort(endpoints[0].begin(), endpoints[0].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); - std::sort(endpoints[1].begin(), endpoints[1].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); - std::sort(endpoints[2].begin(), endpoints[2].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + std::sort(endpoints[0].begin(), endpoints[0].end()); + std::sort(endpoints[1].begin(), endpoints[1].end()); + std::sort(endpoints[2].begin(), endpoints[2].end()); for(int i = 0; i < 3; ++i) delete interval_trees[i]; @@ -265,7 +266,7 @@ void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) for(int i = 0; i < 3; ++i) { dummy.value = old_aabb.min_[i]; - it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); for(; it != endpoints[i].end(); ++it) { if(it->obj == updated_obj && it->minmax == 0) @@ -276,7 +277,7 @@ void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) } dummy.value = old_aabb.max_[i]; - it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy, boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + it = std::lower_bound(endpoints[i].begin(), endpoints[i].end(), dummy); for(; it != endpoints[i].end(); ++it) { if(it->obj == updated_obj && it->minmax == 0) @@ -286,7 +287,7 @@ void IntervalTreeCollisionManager::update(CollisionObject* updated_obj) } } - std::sort(endpoints[i].begin(), endpoints[i].end(), boost::bind(&EndPoint::value, _1) < boost::bind(&EndPoint::value, _2)); + std::sort(endpoints[i].begin(), endpoints[i].end()); } } diff --git a/src/ccd/interpolation/interpolation_factory.cpp b/src/ccd/interpolation/interpolation_factory.cpp index d75865316..48b81f3db 100644 --- a/src/ccd/interpolation/interpolation_factory.cpp +++ b/src/ccd/interpolation/interpolation_factory.cpp @@ -65,7 +65,7 @@ void InterpolationFactory::registerClass(const InterpolationType type, const Cre this->creation_map_[type] = create_function; } -boost::shared_ptr +std::shared_ptr InterpolationFactory::create(const InterpolationType type, const FCL_REAL start_value, const FCL_REAL end_value) { std::map::const_iterator it = creation_map_.find(type); diff --git a/src/ccd/interpolation/interpolation_linear.cpp b/src/ccd/interpolation/interpolation_linear.cpp index 45cb4b3ae..668564236 100644 --- a/src/ccd/interpolation/interpolation_linear.cpp +++ b/src/ccd/interpolation/interpolation_linear.cpp @@ -69,9 +69,9 @@ InterpolationType InterpolationLinear::getType() const return interpolation_linear_type; } -boost::shared_ptr InterpolationLinear::create(FCL_REAL start_value, FCL_REAL end_value) +std::shared_ptr InterpolationLinear::create(FCL_REAL start_value, FCL_REAL end_value) { - return boost::shared_ptr(new InterpolationLinear(start_value, end_value) ); + return std::shared_ptr(new InterpolationLinear(start_value, end_value) ); } void InterpolationLinear::registerToFactory() diff --git a/src/ccd/taylor_matrix.cpp b/src/ccd/taylor_matrix.cpp index b06a64efd..ca3b33403 100644 --- a/src/ccd/taylor_matrix.cpp +++ b/src/ccd/taylor_matrix.cpp @@ -44,7 +44,7 @@ TMatrix3::TMatrix3() { } -TMatrix3::TMatrix3(const boost::shared_ptr& time_interval) +TMatrix3::TMatrix3(const std::shared_ptr& time_interval) { setTimeInterval(time_interval); } @@ -64,7 +64,7 @@ TMatrix3::TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3) } -TMatrix3::TMatrix3(const Matrix3f& m, const boost::shared_ptr& time_interval) +TMatrix3::TMatrix3(const Matrix3f& m, const std::shared_ptr& time_interval) { v_[0] = TVector3(m.getRow(0), time_interval); v_[1] = TVector3(m.getRow(1), time_interval); @@ -320,7 +320,7 @@ FCL_REAL TMatrix3::diameter() const return d; } -void TMatrix3::setTimeInterval(const boost::shared_ptr& time_interval) +void TMatrix3::setTimeInterval(const std::shared_ptr& time_interval) { v_[0].setTimeInterval(time_interval); v_[1].setTimeInterval(time_interval); @@ -334,7 +334,7 @@ void TMatrix3::setTimeInterval(FCL_REAL l, FCL_REAL r) v_[2].setTimeInterval(l, r); } -const boost::shared_ptr& TMatrix3::getTimeInterval() const +const std::shared_ptr& TMatrix3::getTimeInterval() const { return v_[0].getTimeInterval(); } diff --git a/src/ccd/taylor_model.cpp b/src/ccd/taylor_model.cpp index c08b39c10..08d9c2783 100644 --- a/src/ccd/taylor_model.cpp +++ b/src/ccd/taylor_model.cpp @@ -35,12 +35,11 @@ /** \author Jia Pan */ +#include "fcl/math/constants.h" #include "fcl/ccd/taylor_model.h" #include #include #include -#include - namespace fcl { @@ -50,18 +49,18 @@ TaylorModel::TaylorModel() coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; } -TaylorModel::TaylorModel(const boost::shared_ptr& time_interval) : time_interval_(time_interval) +TaylorModel::TaylorModel(const std::shared_ptr& time_interval) : time_interval_(time_interval) { coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0; } -TaylorModel::TaylorModel(FCL_REAL coeff, const boost::shared_ptr& time_interval) : time_interval_(time_interval) +TaylorModel::TaylorModel(FCL_REAL coeff, const std::shared_ptr& time_interval) : time_interval_(time_interval) { coeffs_[0] = coeff; coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0; } -TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr& time_interval) : time_interval_(time_interval) +TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r, const std::shared_ptr& time_interval) : time_interval_(time_interval) { coeffs_[0] = coeffs[0]; coeffs_[1] = coeffs[1]; @@ -71,7 +70,7 @@ TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::sha r_ = r; } -TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr& time_interval) : time_interval_(time_interval) +TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const std::shared_ptr& time_interval) : time_interval_(time_interval) { coeffs_[0] = c0; coeffs_[1] = c1; @@ -387,8 +386,8 @@ void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0) // cos reaches maximum if there exists an integer k in [(w*t0+q0)/2pi, (w*t1+q0)/2pi]; // cos reaches minimum if there exists an integer k in [(w*t0+q0-pi)/2pi, (w*t1+q0-pi)/2pi] - FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * boost::math::constants::pi()); - FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * boost::math::constants::pi()); + FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants::pi); + FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants::pi); if(w > 0) @@ -458,8 +457,8 @@ void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0) // sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi]; // sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi] - FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * boost::math::constants::pi()) - 0.25; - FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * boost::math::constants::pi()) - 0.25; + FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * constants::pi) - 0.25; + FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * constants::pi) - 0.25; if(w > 0) { diff --git a/src/ccd/taylor_vector.cpp b/src/ccd/taylor_vector.cpp index 340901791..9b676b8a6 100644 --- a/src/ccd/taylor_vector.cpp +++ b/src/ccd/taylor_vector.cpp @@ -44,7 +44,7 @@ TVector3::TVector3() { } -TVector3::TVector3(const boost::shared_ptr& time_interval) +TVector3::TVector3(const std::shared_ptr& time_interval) { setTimeInterval(time_interval); } @@ -63,7 +63,7 @@ TVector3::TVector3(const TaylorModel& v1, const TaylorModel& v2, const TaylorMod i_[2] = v3; } -TVector3::TVector3(const Vec3f& v, const boost::shared_ptr& time_interval) +TVector3::TVector3(const Vec3f& v, const std::shared_ptr& time_interval) { i_[0] = TaylorModel(v[0], time_interval); i_[1] = TaylorModel(v[1], time_interval); @@ -239,7 +239,7 @@ TaylorModel TVector3::squareLength() const return i_[0] * i_[0] + i_[1] * i_[1] + i_[2] * i_[2]; } -void TVector3::setTimeInterval(const boost::shared_ptr& time_interval) +void TVector3::setTimeInterval(const std::shared_ptr& time_interval) { i_[0].setTimeInterval(time_interval); i_[1].setTimeInterval(time_interval); @@ -253,7 +253,7 @@ void TVector3::setTimeInterval(FCL_REAL l, FCL_REAL r) i_[2].setTimeInterval(l, r); } -const boost::shared_ptr& TVector3::getTimeInterval() const +const std::shared_ptr& TVector3::getTimeInterval() const { return i_[0].getTimeInterval(); } diff --git a/src/intersect.cpp b/src/intersect.cpp index 66bd4a8ff..be2650a42 100644 --- a/src/intersect.cpp +++ b/src/intersect.cpp @@ -39,7 +39,6 @@ #include #include #include -#include namespace fcl { @@ -1200,7 +1199,7 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, // clamp result so t is on the segment P,A - if((t < 0) || boost::math::isnan(t)) t = 0; else if(t > 1) t = 1; + 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 @@ -1210,13 +1209,13 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, // closest points, otherwise, clamp u, recompute and // clamp t - if((u <= 0) || boost::math::isnan(u)) + if((u <= 0) || std::isnan(u)) { Y = Q; t = A_dot_T / A_dot_A; - if((t <= 0) || boost::math::isnan(t)) + if((t <= 0) || std::isnan(t)) { X = P; VEC = Q - P; @@ -1239,7 +1238,7 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, t = (A_dot_B + A_dot_T) / A_dot_A; - if((t <= 0) || boost::math::isnan(t)) + if((t <= 0) || std::isnan(t)) { X = P; VEC = Y - P; @@ -1261,7 +1260,7 @@ void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, { Y = Q + B * u; - if((t <= 0) || boost::math::isnan(t)) + if((t <= 0) || std::isnan(t)) { X = P; TMP = T.cross(B); diff --git a/src/math/sampling.cpp b/src/math/sampling.cpp index 4838eda44..033ed8f1a 100644 --- a/src/math/sampling.cpp +++ b/src/math/sampling.cpp @@ -3,7 +3,6 @@ #include #include #include -#include namespace fcl { @@ -99,7 +98,7 @@ void RNG::quaternion(double value[4]) { double x0 = uni_(); double r1 = sqrt(1.0 - x0), r2 = sqrt(x0); - double t1 = 2.0 * boost::math::constants::pi() * uni_(), t2 = 2.0 * boost::math::constants::pi() * uni_(); + double t1 = 2.0 * constants::pi * uni_(), t2 = 2.0 * constants::pi * uni_(); double c1 = cos(t1), s1 = sin(t1); double c2 = cos(t2), s2 = sin(t2); value[0] = s1 * r1; @@ -111,9 +110,9 @@ void RNG::quaternion(double value[4]) // From Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning, by James Kuffner, ICRA 2004 void RNG::eulerRPY(double value[3]) { - value[0] = boost::math::constants::pi() * (2.0 * uni_() - 1.0); - value[1] = acos(1.0 - 2.0 * uni_()) - boost::math::constants::pi() / 2.0; - value[2] = boost::math::constants::pi() * (2.0 * uni_() - 1.0); + value[0] = constants::pi * (2.0 * uni_() - 1.0); + value[1] = acos(1.0 - 2.0 * uni_()) - constants::pi / 2.0; + value[2] = constants::pi * (2.0 * uni_() - 1.0); } void RNG::disk(double r_min, double r_max, double& x, double& y) @@ -121,7 +120,7 @@ void RNG::disk(double r_min, double r_max, double& x, double& y) double a = uniform01(); double b = uniform01(); double r = std::sqrt(a * r_max * r_max + (1 - a) * r_min * r_min); - double theta = 2 * boost::math::constants::pi() * b; + double theta = 2 * constants::pi * b; x = r * std::cos(theta); y = r * std::sin(theta); } @@ -133,7 +132,7 @@ void RNG::ball(double r_min, double r_max, double& x, double& y, double& z) double c = uniform01(); double r = std::pow(a * r_max * r_max * r_max + (1 - a) * r_min * r_min * r_min, 1 / 3.0); double theta = std::acos(1 - 2 * b); - double phi = 2 * boost::math::constants::pi() * c; + double phi = 2 * constants::pi * c; double costheta = std::cos(theta); double sintheta = std::sin(theta); diff --git a/src/math/transform.cpp b/src/math/transform.cpp index a2c20b36e..9a60c2c46 100644 --- a/src/math/transform.cpp +++ b/src/math/transform.cpp @@ -35,9 +35,8 @@ /** \author Jia Pan */ - +#include "fcl/math/constants.h" #include "fcl/math/transform.h" -#include namespace fcl { @@ -351,17 +350,17 @@ void Quaternion3f::toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const b = asin(-R(2, 0)); c = atan2(R(2, 1), R(2, 2)); - if(b == boost::math::constants::pi() * 0.5) + if(b == constants::pi * 0.5) { if(a > 0) - a -= boost::math::constants::pi(); + a -= constants::pi; else - a += boost::math::constants::pi(); + a += constants::pi; if(c > 0) - c -= boost::math::constants::pi(); + c -= constants::pi; else - c += boost::math::constants::pi(); + c += constants::pi; } } diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index 11fef9824..e7b8991ab 100755 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -38,7 +38,6 @@ #include "fcl/narrowphase/narrowphase.h" #include "fcl/shape/geometric_shapes_utility.h" #include "fcl/intersect.h" -#include #include namespace fcl @@ -871,7 +870,7 @@ static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[]) avail[i0] = 0; iret[0] = i0; iret++; - const double pi = boost::math::constants::pi(); + const double pi = constants::pi; for(int j = 1; j < m; ++j) { a = j*(2*pi/m) + A[i0]; diff --git a/src/profile.cpp b/src/profile.cpp index f25a8894f..088701c2c 100644 --- a/src/profile.cpp +++ b/src/profile.cpp @@ -86,14 +86,14 @@ void fcl::tools::Profiler::clear(void) void fcl::tools::Profiler::event(const std::string &name, const unsigned int times) { lock_.lock(); - data_[boost::this_thread::get_id()].events[name] += times; + data_[std::this_thread::get_id()].events[name] += times; lock_.unlock(); } void fcl::tools::Profiler::average(const std::string &name, const double value) { lock_.lock(); - AvgInfo &a = data_[boost::this_thread::get_id()].avg[name]; + AvgInfo &a = data_[std::this_thread::get_id()].avg[name]; a.total += value; a.totalSqr += value*value; a.parts++; @@ -103,14 +103,14 @@ void fcl::tools::Profiler::average(const std::string &name, const double value) void fcl::tools::Profiler::begin(const std::string &name) { lock_.lock(); - data_[boost::this_thread::get_id()].time[name].set(); + data_[std::this_thread::get_id()].time[name].set(); lock_.unlock(); } void fcl::tools::Profiler::end(const std::string &name) { lock_.lock(); - data_[boost::this_thread::get_id()].time[name].update(); + data_[std::this_thread::get_id()].time[name].update(); lock_.unlock(); } @@ -126,7 +126,7 @@ void fcl::tools::Profiler::status(std::ostream &out, bool merge) if (merge) { PerThread combined; - for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) + for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) { for (std::map::const_iterator iev = it->second.events.begin() ; iev != it->second.events.end(); ++iev) combined.events[iev->first] += iev->second; @@ -150,7 +150,7 @@ void fcl::tools::Profiler::status(std::ostream &out, bool merge) printThreadInfo(out, combined); } else - for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) + for (std::map::const_iterator it = data_.begin() ; it != data_.end() ; ++it) { out << "Thread " << it->first << ":" << std::endl; printThreadInfo(out, it->second); diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp index 5ec616a78..276475723 100644 --- a/src/shape/geometric_shapes_utility.cpp +++ b/src/shape/geometric_shapes_utility.cpp @@ -94,10 +94,7 @@ std::vector getBoundVertices(const Ellipsoid& ellipsoid, const Transform3 std::vector result(12); - const FCL_REAL phi = (1.0 + sqrt(5.0)) / 2.0; // golden ratio - // TODO: Replace with constexpr when we migrate to C++11 or - // boost::math::constants::phi() if boost version is greater - // than 1.50. + const FCL_REAL phi = (1.0 + std::sqrt(5.0)) / 2.0; // golden ratio const FCL_REAL a = std::sqrt(3.0) / (phi * phi); const FCL_REAL b = phi * a; diff --git a/test/general_test.cpp b/test/general_test.cpp index 1b3399a00..1c362da7a 100644 --- a/test/general_test.cpp +++ b/test/general_test.cpp @@ -10,8 +10,8 @@ using namespace fcl; int main(int argc, char** argv) { - boost::shared_ptr box0(new Box(1,1,1)); - boost::shared_ptr box1(new Box(1,1,1)); + std::shared_ptr box0(new Box(1,1,1)); + std::shared_ptr box1(new Box(1,1,1)); // GJKSolver_indep solver; GJKSolver_libccd solver; Vec3f contact_points; diff --git a/test/test_fcl_broadphase.cpp b/test/test_fcl_broadphase.cpp index 85f3c6485..287dba792 100644 --- a/test/test_fcl_broadphase.cpp +++ b/test/test_fcl_broadphase.cpp @@ -51,7 +51,6 @@ #include #endif -#include #include #include @@ -293,21 +292,21 @@ void generateEnvironments(std::vector& env, double env_scale, for(std::size_t i = 0; i < n; ++i) { Box* box = new Box(5, 10, 20); - env.push_back(new CollisionObject(boost::shared_ptr(box), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(box), transforms[i])); } generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Sphere* sphere = new Sphere(30); - env.push_back(new CollisionObject(boost::shared_ptr(sphere), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(sphere), transforms[i])); } generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Cylinder* cylinder = new Cylinder(10, 40); - env.push_back(new CollisionObject(boost::shared_ptr(cylinder), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(cylinder), transforms[i])); } } @@ -322,7 +321,7 @@ void generateEnvironmentsMesh(std::vector& env, double env_sca { BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f()); - env.push_back(new CollisionObject(boost::shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } generateRandomTransforms(extents, transforms, n); @@ -331,7 +330,7 @@ void generateEnvironmentsMesh(std::vector& env, double env_sca { BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3f(), 16, 16); - env.push_back(new CollisionObject(boost::shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } generateRandomTransforms(extents, transforms, n); @@ -340,7 +339,7 @@ void generateEnvironmentsMesh(std::vector& env, double env_sca { BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3f(), 16, 16); - env.push_back(new CollisionObject(boost::shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } } @@ -360,7 +359,7 @@ void generateSelfDistanceEnvironments(std::vector& env, double int z = i - n_edge * n_edge * x - n_edge * y; Box* box = new Box(single_size, single_size, single_size); - env.push_back(new CollisionObject(boost::shared_ptr(box), + env.push_back(new CollisionObject(std::shared_ptr(box), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -373,7 +372,7 @@ void generateSelfDistanceEnvironments(std::vector& env, double int z = i - n_edge * n_edge * x - n_edge * y; Sphere* sphere = new Sphere(single_size / 2); - env.push_back(new CollisionObject(boost::shared_ptr(sphere), + env.push_back(new CollisionObject(std::shared_ptr(sphere), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -386,7 +385,7 @@ void generateSelfDistanceEnvironments(std::vector& env, double int z = i - n_edge * n_edge * x - n_edge * y; Ellipsoid* ellipsoid = new Ellipsoid(single_size / 2, single_size / 2, single_size / 2); - env.push_back(new CollisionObject(boost::shared_ptr(ellipsoid), + env.push_back(new CollisionObject(std::shared_ptr(ellipsoid), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -399,7 +398,7 @@ void generateSelfDistanceEnvironments(std::vector& env, double int z = i - n_edge * n_edge * x - n_edge * y; Cylinder* cylinder = new Cylinder(single_size / 2, single_size); - env.push_back(new CollisionObject(boost::shared_ptr(cylinder), + env.push_back(new CollisionObject(std::shared_ptr(cylinder), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -412,7 +411,7 @@ void generateSelfDistanceEnvironments(std::vector& env, double int z = i - n_edge * n_edge * x - n_edge * y; Cone* cone = new Cone(single_size / 2, single_size); - env.push_back(new CollisionObject(boost::shared_ptr(cone), + env.push_back(new CollisionObject(std::shared_ptr(cone), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -437,7 +436,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, do Box box(single_size, single_size, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f()); - env.push_back(new CollisionObject(boost::shared_ptr(model), + env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -452,7 +451,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, do Sphere sphere(single_size / 2); BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3f(), 16, 16); - env.push_back(new CollisionObject(boost::shared_ptr(model), + env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -467,7 +466,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, do Ellipsoid ellipsoid(single_size / 2, single_size / 2, single_size / 2); BVHModel* model = new BVHModel(); generateBVHModel(*model, ellipsoid, Transform3f(), 16, 16); - env.push_back(new CollisionObject(boost::shared_ptr(model), + env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -482,7 +481,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, do Cylinder cylinder(single_size / 2, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3f(), 16, 16); - env.push_back(new CollisionObject(boost::shared_ptr(model), + env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -497,7 +496,7 @@ void generateSelfDistanceEnvironmentsMesh(std::vector& env, do Cone cone(single_size / 2, single_size); BVHModel* model = new BVHModel(); generateBVHModel(*model, cone, Transform3f(), 16, 16); - env.push_back(new CollisionObject(boost::shared_ptr(model), + env.push_back(new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x * step_size + delta_size + 0.5 * single_size - env_scale, y * step_size + delta_size + 0.5 * single_size - env_scale, z * step_size + delta_size + 0.5 * single_size - env_scale)))); @@ -1036,7 +1035,7 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s } // update the environment - FCL_REAL delta_angle_max = 10 / 360.0 * 2 * boost::math::constants::pi(); + FCL_REAL delta_angle_max = 10 / 360.0 * 2 * constants::pi; FCL_REAL delta_trans_max = 0.01 * env_scale; for(size_t i = 0; i < env.size(); ++i) { diff --git a/test/test_fcl_bvh_models.cpp b/test/test_fcl_bvh_models.cpp index 397046f5d..68d4f9e34 100644 --- a/test/test_fcl_bvh_models.cpp +++ b/test/test_fcl_bvh_models.cpp @@ -50,7 +50,7 @@ using namespace fcl; template void testBVHModelPointCloud() { - boost::shared_ptr > model(new BVHModel); + std::shared_ptr > model(new BVHModel); if (model->getNodeType() != BV_AABB && model->getNodeType() != BV_KDOP16 @@ -101,7 +101,7 @@ void testBVHModelPointCloud() template void testBVHModelTriangles() { - boost::shared_ptr > model(new BVHModel); + std::shared_ptr > model(new BVHModel); Box box; double a = box.side[0]; @@ -155,7 +155,7 @@ void testBVHModelTriangles() template void testBVHModelSubModel() { - boost::shared_ptr > model(new BVHModel); + std::shared_ptr > model(new BVHModel); Box box; double a = box.side[0]; diff --git a/test/test_fcl_capsule_box_1.cpp b/test/test_fcl_capsule_box_1.cpp index 4a4ae4f01..3e864b67d 100644 --- a/test/test_fcl_capsule_box_1.cpp +++ b/test/test_fcl_capsule_box_1.cpp @@ -48,7 +48,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { - typedef boost::shared_ptr CollisionGeometryPtr_t; + typedef std::shared_ptr CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); // Box of size 1 by 2 by 4 diff --git a/test/test_fcl_capsule_box_2.cpp b/test/test_fcl_capsule_box_2.cpp index 273423d3c..1de5197b7 100644 --- a/test/test_fcl_capsule_box_2.cpp +++ b/test/test_fcl_capsule_box_2.cpp @@ -48,7 +48,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box) { - typedef boost::shared_ptr CollisionGeometryPtr_t; + typedef std::shared_ptr CollisionGeometryPtr_t; // Capsule of radius 2 and of height 4 CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.)); // Box of size 1 by 2 by 4 diff --git a/test/test_fcl_capsule_capsule.cpp b/test/test_fcl_capsule_capsule.cpp index 2c8359cc4..ebb8c3055 100644 --- a/test/test_fcl_capsule_capsule.cpp +++ b/test/test_fcl_capsule_capsule.cpp @@ -37,8 +37,8 @@ #define BOOST_TEST_MODULE "FCL_CAPSULE_CAPSULE" #include -#include +#include "fcl/math/constants.h" #include "fcl/collision.h" #include "fcl/shape/geometric_shapes.h" #include "fcl/narrowphase/narrowphase.h" @@ -126,7 +126,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ) BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2) { - const FCL_REAL Pi = boost::math::constants::pi(); + const FCL_REAL Pi = constants::pi; GJKSolver_indep solver; Capsule s1(5, 10); diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp index 7b5c64077..74d7d4cba 100755 --- a/test/test_fcl_geometric_shapes.cpp +++ b/test/test_fcl_geometric_shapes.cpp @@ -59,7 +59,7 @@ BOOST_AUTO_TEST_CASE(sphere_shape) { const double tol = 1e-12; const double radius = 5.0; - const double pi = boost::math::constants::pi(); + const double pi = constants::pi; Sphere s(radius); diff --git a/test/test_fcl_octomap.cpp b/test/test_fcl_octomap.cpp index 0d4b49cc8..c31c8e43c 100644 --- a/test/test_fcl_octomap.cpp +++ b/test/test_fcl_octomap.cpp @@ -209,14 +209,14 @@ void octomap_collision_test_BVH(std::size_t n, bool exhaustive) loadOBJFile((path / "env.obj").string().c_str(), p1, t1); BVHModel* m1 = new BVHModel(); - boost::shared_ptr m1_ptr(m1); + std::shared_ptr m1_ptr(m1); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); - OcTree* tree = new OcTree(boost::shared_ptr(generateOcTree())); - boost::shared_ptr tree_ptr(tree); + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); + std::shared_ptr tree_ptr(tree); std::vector transforms; FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10}; @@ -275,14 +275,14 @@ void octomap_distance_test_BVH(std::size_t n) loadOBJFile((path / "env.obj").string().c_str(), p1, t1); BVHModel* m1 = new BVHModel(); - boost::shared_ptr m1_ptr(m1); + std::shared_ptr m1_ptr(m1); m1->beginModel(); m1->addSubModel(p1, t1); m1->endModel(); - OcTree* tree = new OcTree(boost::shared_ptr(generateOcTree())); - boost::shared_ptr tree_ptr(tree); + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); + std::shared_ptr tree_ptr(tree); std::vector transforms; FCL_REAL extents[] = {-10, -10, 10, 10, 10, 10}; @@ -331,8 +331,8 @@ void octomap_cost_test(double env_scale, std::size_t env_size, std::size_t num_m else generateEnvironments(env, env_scale, env_size); - OcTree* tree = new OcTree(boost::shared_ptr(generateOcTree())); - CollisionObject tree_obj((boost::shared_ptr(tree))); + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); + CollisionObject tree_obj((std::shared_ptr(tree))); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(env); @@ -451,8 +451,8 @@ void octomap_collision_test(double env_scale, std::size_t env_size, bool exhaust else generateEnvironments(env, env_scale, env_size); - OcTree* tree = new OcTree(boost::shared_ptr(generateOcTree())); - CollisionObject tree_obj((boost::shared_ptr(tree))); + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); + CollisionObject tree_obj((std::shared_ptr(tree))); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(env); @@ -550,8 +550,8 @@ void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh else generateEnvironments(env, env_scale, env_size); - OcTree* tree = new OcTree(boost::shared_ptr(generateOcTree())); - CollisionObject tree_obj((boost::shared_ptr(tree))); + OcTree* tree = new OcTree(std::shared_ptr(generateOcTree())); + CollisionObject tree_obj((std::shared_ptr(tree))); DynamicAABBTreeCollisionManager* manager = new DynamicAABBTreeCollisionManager(); manager->registerObjects(env); @@ -645,7 +645,7 @@ void generateBoxesFromOctomap(std::vector& boxes, OcTree& tree Box* box = new Box(size, size, size); box->cost_density = cost; box->threshold_occupied = threshold; - CollisionObject* obj = new CollisionObject(boost::shared_ptr(box), Transform3f(Vec3f(x, y, z))); + CollisionObject* obj = new CollisionObject(std::shared_ptr(box), Transform3f(Vec3f(x, y, z))); boxes.push_back(obj); } @@ -662,21 +662,21 @@ void generateEnvironments(std::vector& env, double env_scale, for(std::size_t i = 0; i < n; ++i) { Box* box = new Box(5, 10, 20); - env.push_back(new CollisionObject(boost::shared_ptr(box), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(box), transforms[i])); } generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Sphere* sphere = new Sphere(30); - env.push_back(new CollisionObject(boost::shared_ptr(sphere), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(sphere), transforms[i])); } generateRandomTransforms(extents, transforms, n); for(std::size_t i = 0; i < n; ++i) { Cylinder* cylinder = new Cylinder(10, 40); - env.push_back(new CollisionObject(boost::shared_ptr(cylinder), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(cylinder), transforms[i])); } } @@ -700,7 +700,7 @@ void generateBoxesFromOctomapMesh(std::vector& boxes, OcTree& generateBVHModel(*model, box, Transform3f()); model->cost_density = cost; model->threshold_occupied = threshold; - CollisionObject* obj = new CollisionObject(boost::shared_ptr(model), Transform3f(Vec3f(x, y, z))); + CollisionObject* obj = new CollisionObject(std::shared_ptr(model), Transform3f(Vec3f(x, y, z))); boxes.push_back(obj); } @@ -718,7 +718,7 @@ void generateEnvironmentsMesh(std::vector& env, double env_sca { BVHModel* model = new BVHModel(); generateBVHModel(*model, box, Transform3f()); - env.push_back(new CollisionObject(boost::shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } generateRandomTransforms(extents, transforms, n); @@ -727,7 +727,7 @@ void generateEnvironmentsMesh(std::vector& env, double env_sca { BVHModel* model = new BVHModel(); generateBVHModel(*model, sphere, Transform3f(), 16, 16); - env.push_back(new CollisionObject(boost::shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } generateRandomTransforms(extents, transforms, n); @@ -736,7 +736,7 @@ void generateEnvironmentsMesh(std::vector& env, double env_sca { BVHModel* model = new BVHModel(); generateBVHModel(*model, cylinder, Transform3f(), 16, 16); - env.push_back(new CollisionObject(boost::shared_ptr(model), transforms[i])); + env.push_back(new CollisionObject(std::shared_ptr(model), transforms[i])); } } diff --git a/test/test_fcl_sphere_capsule.cpp b/test/test_fcl_sphere_capsule.cpp index 354192963..57f3da1f4 100644 --- a/test/test_fcl_sphere_capsule.cpp +++ b/test/test_fcl_sphere_capsule.cpp @@ -38,14 +38,11 @@ #define BOOST_TEST_MODULE "FCL_SPHERE_CAPSULE" #include +#include "fcl/math/constants.h" #include "fcl/collision.h" #include "fcl/shape/geometric_shapes.h" #include "fcl/narrowphase/narrowphase.h" -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - using namespace fcl; BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_z) @@ -100,7 +97,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_capsule_rotated) Capsule capsule (50, 200.); Matrix3f rotation; - rotation.setEulerZYX (M_PI * 0.5, 0., 0.); + rotation.setEulerZYX (constants::pi * 0.5, 0., 0.); Transform3f capsule_transform (rotation, Vec3f (150., 0., 0.)); BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL)); @@ -141,7 +138,7 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated) Capsule capsule (50, 200.); Matrix3f rotation; - rotation.setEulerZYX (M_PI * 0.5, 0., 0.); + rotation.setEulerZYX (constants::pi * 0.5, 0., 0.); Transform3f capsule_transform (rotation, Vec3f (0., 50., 75)); std::vector contacts;