From 8742058f38c1a56501f32f5c7a8a7088870f3060 Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Thu, 24 Mar 2016 11:41:14 -0500 Subject: [PATCH 1/2] Remove more boost usage and replace with C++11 equivalents. Note: BVH_Model has a copy constructor, so having it derive from boost::noncopyable makes no sense. We can delete the copy assignment operator, if desired. Only remaining boost usage in include/ and src/ is boost::dynamic_bitset. Use std::vector instead? --- include/fcl/BVH/BVH_model.h | 4 +- include/fcl/broadphase/interval_tree.h | 1 - .../ccd/interpolation/interpolation_factory.h | 4 +- include/fcl/data_types.h | 10 ++-- include/fcl/intersect.h | 1 - include/fcl/knn/greedy_kcenters.h | 2 +- include/fcl/knn/nearest_neighbors.h | 3 +- include/fcl/knn/nearest_neighbors_GNAT.h | 10 ++-- include/fcl/math/sampling.h | 49 ++++++++---------- include/fcl/math/transform.h | 4 +- include/fcl/math/vec_nf.h | 1 - include/fcl/octree.h | 8 +-- include/fcl/profile.h | 20 ++++---- .../fcl/shape/geometric_shape_to_BVH_model.h | 1 - include/fcl/traversal/traversal_node_bvhs.h | 1 - src/BVH/BVH_model.cpp | 1 - src/articulated_model/model.cpp | 1 - src/articulated_model/model_config.cpp | 10 ++-- .../interpolation/interpolation_factory.cpp | 10 +--- src/math/sampling.cpp | 50 +++++++++---------- src/math/transform.cpp | 3 +- src/profile.cpp | 4 +- test/general_test.cpp | 3 +- test/test_fcl_octomap.cpp | 4 +- test/test_fcl_simple.cpp | 4 +- 25 files changed, 90 insertions(+), 119 deletions(-) diff --git a/include/fcl/BVH/BVH_model.h b/include/fcl/BVH/BVH_model.h index e5a8c7cfb..b011d4ade 100644 --- a/include/fcl/BVH/BVH_model.h +++ b/include/fcl/BVH/BVH_model.h @@ -45,15 +45,13 @@ #include "fcl/BVH/BV_fitter.h" #include #include -#include namespace fcl { /// @brief A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as a degraded version of mesh) template -class BVHModel : public CollisionGeometry, - private boost::noncopyable +class BVHModel : public CollisionGeometry { public: diff --git a/include/fcl/broadphase/interval_tree.h b/include/fcl/broadphase/interval_tree.h index 9b32948bb..449eaf96c 100644 --- a/include/fcl/broadphase/interval_tree.h +++ b/include/fcl/broadphase/interval_tree.h @@ -47,7 +47,6 @@ namespace fcl /// @brief Interval trees implemented using red-black-trees as described in /// the book Introduction_To_Algorithms_ by Cormen, Leisserson, and Rivest. -/// Can be replaced in part by boost::icl::interval_set, which is only supported after boost 1.46 and does not support delete node routine. struct SimpleInterval { public: diff --git a/include/fcl/ccd/interpolation/interpolation_factory.h b/include/fcl/ccd/interpolation/interpolation_factory.h index ac5fcf2c1..459330208 100644 --- a/include/fcl/ccd/interpolation/interpolation_factory.h +++ b/include/fcl/ccd/interpolation/interpolation_factory.h @@ -44,7 +44,7 @@ #include #include -#include +#include namespace fcl { @@ -52,7 +52,7 @@ namespace fcl class InterpolationFactory { public: - typedef boost::function(FCL_REAL, FCL_REAL)> CreateFunction; + typedef std::function(FCL_REAL, FCL_REAL)> CreateFunction; public: void registerClass(const InterpolationType type, const CreateFunction create_function); diff --git a/include/fcl/data_types.h b/include/fcl/data_types.h index 88e995d49..64181944b 100644 --- a/include/fcl/data_types.h +++ b/include/fcl/data_types.h @@ -39,16 +39,16 @@ #define FCL_DATA_TYPES_H #include -#include +#include namespace fcl { typedef double FCL_REAL; -typedef boost::uint64_t FCL_INT64; -typedef boost::int64_t FCL_UINT64; -typedef boost::uint32_t FCL_UINT32; -typedef boost::int32_t FCL_INT32; +typedef std::uint_fast64_t FCL_INT64; +typedef std::int_fast64_t FCL_UINT64; +typedef std::uint_fast32_t FCL_UINT32; +typedef std::int_fast32_t FCL_INT32; /// @brief Triangle with 3 indices for points class Triangle diff --git a/include/fcl/intersect.h b/include/fcl/intersect.h index f7703d417..b05642ff9 100644 --- a/include/fcl/intersect.h +++ b/include/fcl/intersect.h @@ -39,7 +39,6 @@ #define FCL_INTERSECT_H #include "fcl/math/transform.h" -#include namespace fcl { diff --git a/include/fcl/knn/greedy_kcenters.h b/include/fcl/knn/greedy_kcenters.h index 030327760..9bb6c8be1 100644 --- a/include/fcl/knn/greedy_kcenters.h +++ b/include/fcl/knn/greedy_kcenters.h @@ -50,7 +50,7 @@ class GreedyKCenters { public: /// @brief The definition of a distance function - typedef boost::function DistanceFunction; + typedef std::function DistanceFunction; GreedyKCenters(void) { diff --git a/include/fcl/knn/nearest_neighbors.h b/include/fcl/knn/nearest_neighbors.h index a789f280c..802af74cc 100644 --- a/include/fcl/knn/nearest_neighbors.h +++ b/include/fcl/knn/nearest_neighbors.h @@ -40,7 +40,6 @@ #include #include -#include namespace fcl { @@ -51,7 +50,7 @@ class NearestNeighbors public: /// @brief The definition of a distance function - typedef boost::function DistanceFunction; + typedef std::function DistanceFunction; NearestNeighbors(void) { diff --git a/include/fcl/knn/nearest_neighbors_GNAT.h b/include/fcl/knn/nearest_neighbors_GNAT.h index e6e8196ea..ca527c9e3 100644 --- a/include/fcl/knn/nearest_neighbors_GNAT.h +++ b/include/fcl/knn/nearest_neighbors_GNAT.h @@ -40,7 +40,7 @@ #include "fcl/knn/nearest_neighbors.h" #include "fcl/knn/greedy_kcenters.h" #include "fcl/exception.h" -#include +#include #include #include @@ -239,7 +239,7 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> if (!gnat.removed_.empty()) { out << "Elements marked for removal:\n"; - for (typename boost::unordered_set::const_iterator it = gnat.removed_.begin(); + for (typename std::unordered_set::const_iterator it = gnat.removed_.begin(); it != gnat.removed_.end(); it++) out << **it << '\t'; out << std::endl; @@ -252,12 +252,12 @@ class NearestNeighborsGNAT : public NearestNeighbors<_T> void integrityCheck() { std::vector<_T> lst; - boost::unordered_set tmp; + std::unordered_set tmp; // get all elements, including those marked for removal removed_.swap(tmp); list(lst); // check if every element marked for removal is also in the tree - for (typename boost::unordered_set::iterator it=tmp.begin(); it!=tmp.end(); it++) + for (typename std::unordered_set::iterator it=tmp.begin(); it!=tmp.end(); it++) { unsigned int i; for (i=0; i /// \brief The data structure used to split data into subtrees. GreedyKCenters<_T> pivotSelector_; /// \brief Cache of removed elements. - boost::unordered_set removed_; + std::unordered_set removed_; }; } diff --git a/include/fcl/math/sampling.h b/include/fcl/math/sampling.h index 3c2a9faed..fb69a7847 100644 --- a/include/fcl/math/sampling.h +++ b/include/fcl/math/sampling.h @@ -1,11 +1,8 @@ #ifndef FCL_MATH_SAMPLING_H #define FCL_MATH_SAMPLING_H -#include -#include -#include -#include -#include +#include +#include #include "fcl/math/constants.h" #include "fcl/math/vec_nf.h" #include "fcl/math/transform.h" @@ -30,14 +27,14 @@ class RNG /// @brief Generate a random real between 0 and 1 double uniform01() { - return uni_(); + return uniDist_(generator_); } /// @brief Generate a random real within given bounds: [\e lower_bound, \e upper_bound) double uniformReal(double lower_bound, double upper_bound) { assert(lower_bound <= upper_bound); - return (upper_bound - lower_bound) * uni_() + lower_bound; + return (upper_bound - lower_bound) * uniDist_(generator_) + lower_bound; } /// @brief Generate a random integer within given bounds: [\e lower_bound, \e upper_bound] @@ -50,19 +47,19 @@ class RNG /// @brief Generate a random boolean bool uniformBool() { - return uni_() <= 0.5; + return uniDist_(generator_) <= 0.5; } /// @brief Generate a random real using a normal distribution with mean 0 and variance 1 double gaussian01() { - return normal_(); + return normalDist_(generator_); } /// @brief Generate a random real using a normal distribution with given mean and variance double gaussian(double mean, double stddev) { - return normal_() * stddev + mean; + return normalDist_(generator_) * stddev + mean; } /// @brief Generate a random real using a half-normal distribution. The value is within specified bounds [\e r_min, \e r_max], but with a bias towards \e r_max. The function is implemended using a Gaussian distribution with mean at \e r_max - \e r_min. The distribution is 'folded' around \e r_max axis towards \e r_min. The variance of the distribution is (\e r_max - \e r_min) / \e focus. The higher the focus, the more probable it is that generated numbers are close to \e r_max. @@ -84,18 +81,16 @@ class RNG void ball(double r_min, double r_max, double& x, double& y, double& z); /// @brief Set the seed for random number generation. Use this function to ensure the same sequence of random numbers is generated. - static void setSeed(boost::uint32_t seed); + static void setSeed(std::uint_fast32_t seed); /// @brief Get the seed used for random number generation. Passing the returned value to setSeed() at a subsequent execution of the code will ensure deterministic (repeatable) behaviour. Useful for debugging. - static boost::uint32_t getSeed(); + static std::uint_fast32_t getSeed(); private: - boost::mt19937 generator_; - boost::uniform_real<> uniDist_; - boost::normal_distribution<> normalDist_; - boost::variate_generator > uni_; - boost::variate_generator > normal_; + std::mt19937 generator_; + std::uniform_real_distribution<> uniDist_; + std::normal_distribution<> normalDist_; }; @@ -160,8 +155,8 @@ class SamplerSE2 : public SamplerBase {} SamplerSE2(FCL_REAL x_min, FCL_REAL x_max, - FCL_REAL y_min, FCL_REAL y_max) : lower_bound(boost::assign::list_of(x_min)(y_min).convert_to_container >()), - upper_bound(boost::assign::list_of(x_max)(y_max).convert_to_container >()) + FCL_REAL y_min, FCL_REAL y_max) : lower_bound(std::vector({x_min, y_min})), + upper_bound(std::vector({x_max, y_max})) {} @@ -248,8 +243,8 @@ class SamplerSE3Euler : public SamplerBase {} SamplerSE3Euler(const Vec3f& lower_bound_, - const Vec3f& upper_bound_) : lower_bound(boost::assign::list_of(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container >()), - upper_bound(boost::assign::list_of(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container >()) + const Vec3f& upper_bound_) : lower_bound(std::vector({lower_bound_[0], lower_bound_[1], lower_bound_[2]})), + upper_bound(std::vector({upper_bound_[0], upper_bound_[1], upper_bound_[2]})) {} void setBound(const Vecnf<3>& lower_bound_, @@ -263,8 +258,8 @@ class SamplerSE3Euler : public SamplerBase void setBound(const Vec3f& lower_bound_, const Vec3f& upper_bound_) { - lower_bound = Vecnf<3>(boost::assign::list_of(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container >()); - upper_bound = Vecnf<3>(boost::assign::list_of(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container >()); + lower_bound = Vecnf<3>(std::vector({lower_bound_[0], lower_bound_[1], lower_bound_[2]})); + upper_bound = Vecnf<3>(std::vector({upper_bound_[0], upper_bound_[1], upper_bound_[2]})); } void getBound(Vecnf<3>& lower_bound_, @@ -319,8 +314,8 @@ class SamplerSE3Quat : public SamplerBase {} SamplerSE3Quat(const Vec3f& lower_bound_, - const Vec3f& upper_bound_) : lower_bound(boost::assign::list_of(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container >()), - upper_bound(boost::assign::list_of(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container >()) + const Vec3f& upper_bound_) : lower_bound(std::vector({lower_bound_[0], lower_bound_[1], lower_bound_[2]})), + upper_bound(std::vector({upper_bound_[0], upper_bound_[1], upper_bound_[2]})) {} @@ -335,8 +330,8 @@ class SamplerSE3Quat : public SamplerBase void setBound(const Vec3f& lower_bound_, const Vec3f& upper_bound_) { - lower_bound = Vecnf<3>(boost::assign::list_of(lower_bound_[0])(lower_bound_[1])(lower_bound_[2]).convert_to_container >()); - upper_bound = Vecnf<3>(boost::assign::list_of(upper_bound_[0])(upper_bound_[1])(upper_bound_[2]).convert_to_container >()); + lower_bound = Vecnf<3>(std::vector({lower_bound_[0], lower_bound_[1], lower_bound_[2]})); + upper_bound = Vecnf<3>(std::vector({upper_bound_[0], upper_bound_[1], upper_bound_[2]})); } diff --git a/include/fcl/math/transform.h b/include/fcl/math/transform.h index 6fd7f01a4..c4fbe46f5 100644 --- a/include/fcl/math/transform.h +++ b/include/fcl/math/transform.h @@ -40,7 +40,7 @@ #define FCL_TRANSFORM_H #include "fcl/math/matrix_3f.h" -#include +#include namespace fcl { @@ -182,7 +182,7 @@ static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q) /// @brief Simple transform class used locally by InterpMotion class Transform3f { - boost::mutex lock_; + std::mutex lock_; /// @brief Whether matrix cache is set mutable bool matrix_set; diff --git a/include/fcl/math/vec_nf.h b/include/fcl/math/vec_nf.h index db3d416fa..a44d519c4 100644 --- a/include/fcl/math/vec_nf.h +++ b/include/fcl/math/vec_nf.h @@ -5,7 +5,6 @@ #include #include #include -#include #include #include "fcl/data_types.h" diff --git a/include/fcl/octree.h b/include/fcl/octree.h index 7b3c6815d..4271e822c 100644 --- a/include/fcl/octree.h +++ b/include/fcl/octree.h @@ -41,7 +41,7 @@ #include -#include +#include #include #include "fcl/BV/AABB.h" @@ -134,9 +134,9 @@ class OcTree : public CollisionGeometry /// @brief transform the octree into a bunch of boxes; uncertainty information is kept in the boxes. However, we /// only keep the occupied boxes (i.e., the boxes whose occupied probability is higher enough). - inline std::vector > toBoxes() const + inline std::vector > toBoxes() const { - std::vector > boxes; + std::vector > boxes; boxes.reserve(tree->size() / 2); for(octomap::OcTree::iterator it = tree->begin(tree->getTreeDepth()), end = tree->end(); it != end; @@ -152,7 +152,7 @@ class OcTree : public CollisionGeometry FCL_REAL c = (*it).getOccupancy(); FCL_REAL t = tree->getOccupancyThres(); - boost::array box = {{x, y, z, size, c, t}}; + std::array box = {{x, y, z, size, c, t}}; boxes.push_back(box); } } diff --git a/include/fcl/profile.h b/include/fcl/profile.h index 9001fc94e..3d19ea1bd 100644 --- a/include/fcl/profile.h +++ b/include/fcl/profile.h @@ -60,8 +60,7 @@ #include #include #include -#include -#include +#include namespace fcl { @@ -71,15 +70,15 @@ namespace time { /// @brief Representation of a point in time -typedef boost::posix_time::ptime point; +typedef std::chrono::system_clock::time_point point; /// @brief Representation of a time duration -typedef boost::posix_time::time_duration duration; +typedef std::chrono::system_clock::duration duration; /// @brief Get the current time point inline point now(void) { - return boost::posix_time::microsec_clock::universal_time(); + return std::chrono::system_clock::now(); } /// @brief Return the time duration representing a given number of seconds @@ -87,13 +86,13 @@ inline duration seconds(double sec) { long s = (long)sec; long us = (long)((sec - s) * 1000000); - return boost::posix_time::seconds(s) + boost::posix_time::microseconds(us); + return std::chrono::seconds(s) + std::chrono::microseconds(us); } /// @brief Return the number of seconds that a time duration represents inline double seconds(const duration &d) { - return (double)d.total_microseconds() / 1000000.0; + return std::chrono::duration(d).count(); } } @@ -106,9 +105,12 @@ namespace tools /// external profiling tools in that it allows the user to count /// time spent in various bits of code (sub-function granularity) /// or count how many times certain pieces of code are executed. -class Profiler : private boost::noncopyable +class Profiler { public: + // non-copyable + Profiler(const Profiler&) = delete; + Profiler& operator=(const Profiler&) = delete; /// @brief This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of scope. class ScopedBlock @@ -267,7 +269,7 @@ class Profiler : private boost::noncopyable /// @brief Information about time spent in a section of the code struct TimeInfo { - TimeInfo(void) : total(0, 0, 0, 0), shortest(boost::posix_time::pos_infin), longest(boost::posix_time::neg_infin), parts(0) + TimeInfo(void) : total(time::seconds(0.)), shortest(time::duration::max()), longest(time::duration::min()), parts(0) { } diff --git a/include/fcl/shape/geometric_shape_to_BVH_model.h b/include/fcl/shape/geometric_shape_to_BVH_model.h index c26a66115..0fbdeb829 100644 --- a/include/fcl/shape/geometric_shape_to_BVH_model.h +++ b/include/fcl/shape/geometric_shape_to_BVH_model.h @@ -41,7 +41,6 @@ #include "fcl/shape/geometric_shapes.h" #include "fcl/BVH/BVH_model.h" -#include namespace fcl { diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h index 544cfb130..7032ed866 100644 --- a/include/fcl/traversal/traversal_node_bvhs.h +++ b/include/fcl/traversal/traversal_node_bvhs.h @@ -47,7 +47,6 @@ #include "fcl/intersect.h" #include "fcl/ccd/motion.h" -#include #include #include #include diff --git a/src/BVH/BVH_model.cpp b/src/BVH/BVH_model.cpp index 796ab7465..048982b98 100644 --- a/src/BVH/BVH_model.cpp +++ b/src/BVH/BVH_model.cpp @@ -45,7 +45,6 @@ namespace fcl template BVHModel::BVHModel(const BVHModel& other) : CollisionGeometry(other), - boost::noncopyable(), num_tris(other.num_tris), num_vertices(other.num_vertices), build_state(other.build_state), diff --git a/src/articulated_model/model.cpp b/src/articulated_model/model.cpp index 693c9dc3a..cc8944b68 100644 --- a/src/articulated_model/model.cpp +++ b/src/articulated_model/model.cpp @@ -37,7 +37,6 @@ #include "fcl/articulated_model/model.h" #include "fcl/articulated_model/model_config.h" -#include namespace fcl { diff --git a/src/articulated_model/model_config.cpp b/src/articulated_model/model_config.cpp index 514d6e7db..3e907ff4c 100644 --- a/src/articulated_model/model_config.cpp +++ b/src/articulated_model/model_config.cpp @@ -38,11 +38,7 @@ #include "fcl/articulated_model/model_config.h" #include "fcl/articulated_model/joint.h" #include - -// Define for boost version < 1.47 -#ifndef BOOST_ASSERT_MSG -#define BOOST_ASSERT_MSG(expr, msg) ((void)0) -#endif +#include namespace fcl { @@ -63,7 +59,7 @@ ModelConfig::ModelConfig(std::map > joints_m JointConfig ModelConfig::getJointConfigByJointName(const std::string& joint_name) const { std::map::const_iterator it = joint_cfgs_map_.find(joint_name); - BOOST_ASSERT_MSG((it != joint_cfgs_map_.end()), "Joint name not valid"); + assert(it != joint_cfgs_map_.end()); return it->second; } @@ -71,7 +67,7 @@ JointConfig ModelConfig::getJointConfigByJointName(const std::string& joint_name JointConfig& ModelConfig::getJointConfigByJointName(const std::string& joint_name) { std::map::iterator it = joint_cfgs_map_.find(joint_name); - BOOST_ASSERT_MSG((it != joint_cfgs_map_.end()), "Joint name not valid"); + assert(it != joint_cfgs_map_.end()); return it->second; } diff --git a/src/ccd/interpolation/interpolation_factory.cpp b/src/ccd/interpolation/interpolation_factory.cpp index 48b81f3db..527b7d707 100644 --- a/src/ccd/interpolation/interpolation_factory.cpp +++ b/src/ccd/interpolation/interpolation_factory.cpp @@ -37,13 +37,7 @@ #include "fcl/ccd/interpolation/interpolation_factory.h" #include "fcl/ccd/interpolation/interpolation_linear.h" - -#include - -// Define for boost version < 1.47 -#ifndef BOOST_ASSERT_MSG -#define BOOST_ASSERT_MSG(expr, msg) ((void)0) -#endif +#include namespace fcl { @@ -70,7 +64,7 @@ InterpolationFactory::create(const InterpolationType type, const FCL_REAL start_ { std::map::const_iterator it = creation_map_.find(type); - BOOST_ASSERT_MSG((it != creation_map_.end()), "CreateFunction wasn't found."); + assert(it != creation_map_.end()); return (it->second)(start_value, end_value); } diff --git a/src/math/sampling.cpp b/src/math/sampling.cpp index 033ed8f1a..46850bad7 100644 --- a/src/math/sampling.cpp +++ b/src/math/sampling.cpp @@ -1,33 +1,32 @@ #include "fcl/math/sampling.h" -#include -#include -#include -#include +#include +#include namespace fcl { /// The seed the user asked for (cannot be 0) -static boost::uint32_t userSetSeed = 0; +static std::uint_fast32_t userSetSeed = 0; /// Flag indicating whether the first seed has already been generated or not static bool firstSeedGenerated = false; /// The value of the first seed -static boost::uint32_t firstSeedValue = 0; +static std::uint_fast32_t firstSeedValue = 0; /// Compute the first seed to be used; this function should be called only once -static boost::uint32_t firstSeed() +static std::uint_fast32_t firstSeed() { - static boost::mutex fsLock; - boost::mutex::scoped_lock slock(fsLock); + static std::mutex fsLock; + std::unique_lock slock(fsLock); if(firstSeedGenerated) return firstSeedValue; if(userSetSeed != 0) firstSeedValue = userSetSeed; - else firstSeedValue = (boost::uint32_t)(boost::posix_time::microsec_clock::universal_time() - boost::posix_time::ptime(boost::date_time::min_date_time)).total_microseconds(); + else firstSeedValue = (std::uint_fast32_t)std::chrono::duration_cast( + std::chrono::system_clock::now() - std::chrono::system_clock::time_point()).count(); firstSeedGenerated = true; return firstSeedValue; @@ -36,22 +35,21 @@ static boost::uint32_t firstSeed() /// We use a different random number generator for the seeds of the /// Other random generators. The root seed is from the number of /// nano-seconds in the current time. -static boost::uint32_t nextSeed() +static std::uint_fast32_t nextSeed() { - static boost::mutex rngMutex; - boost::mutex::scoped_lock slock(rngMutex); - static boost::lagged_fibonacci607 sGen(firstSeed()); - static boost::uniform_int<> sDist(1, 1000000000); - static boost::variate_generator > s(sGen, sDist); - return s(); + static std::mutex rngMutex; + std::unique_lock slock(rngMutex); + static std::ranlux24_base sGen; + static std::uniform_int_distribution<> sDist(1, 1000000000); + return sDist(sGen); } -boost::uint32_t RNG::getSeed() +std::uint_fast32_t RNG::getSeed() { return firstSeed(); } -void RNG::setSeed(boost::uint32_t seed) +void RNG::setSeed(std::uint_fast32_t seed) { if(firstSeedGenerated) { @@ -68,9 +66,7 @@ void RNG::setSeed(boost::uint32_t seed) RNG::RNG() : generator_(nextSeed()), uniDist_(0, 1), - normalDist_(0, 1), - uni_(generator_, uniDist_), - normal_(generator_, normalDist_) + normalDist_(0, 1) { } @@ -96,9 +92,9 @@ int RNG::halfNormalInt(int r_min, int r_max, double focus) // pg. 124-132 void RNG::quaternion(double value[4]) { - double x0 = uni_(); + double x0 = uniDist_(generator_); double r1 = sqrt(1.0 - x0), r2 = sqrt(x0); - double t1 = 2.0 * constants::pi * uni_(), t2 = 2.0 * constants::pi * uni_(); + double t1 = 2.0 * constants::pi * uniDist_(generator_), t2 = 2.0 * constants::pi * uniDist_(generator_); double c1 = cos(t1), s1 = sin(t1); double c2 = cos(t2), s2 = sin(t2); value[0] = s1 * r1; @@ -110,9 +106,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] = 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); + value[0] = constants::pi * (2.0 * uniDist_(generator_) - 1.0); + value[1] = acos(1.0 - 2.0 * uniDist_(generator_)) - constants::pi / 2.0; + value[2] = constants::pi * (2.0 * uniDist_(generator_) - 1.0); } void RNG::disk(double r_min, double r_max, double& x, double& y) diff --git a/src/math/transform.cpp b/src/math/transform.cpp index 9a60c2c46..2a817d343 100644 --- a/src/math/transform.cpp +++ b/src/math/transform.cpp @@ -37,6 +37,7 @@ #include "fcl/math/constants.h" #include "fcl/math/transform.h" +#include namespace fcl { @@ -410,7 +411,7 @@ Vec3f Quaternion3f::getRow(std::size_t i) const const Matrix3f& Transform3f::getRotationInternal() const { - boost::mutex::scoped_lock slock(const_cast(lock_)); + std::unique_lock slock(const_cast(lock_)); if(!matrix_set) { q.toRotation(R); diff --git a/src/profile.cpp b/src/profile.cpp index 088701c2c..eb0b8d24e 100644 --- a/src/profile.cpp +++ b/src/profile.cpp @@ -37,7 +37,7 @@ /** \author Ioan Sucan */ #include "fcl/profile.h" - +#include fcl::tools::Profiler& fcl::tools::Profiler::Instance(void) { @@ -222,7 +222,7 @@ void fcl::tools::Profiler::printThreadInfo(std::ostream &out, const PerThread &d { const AvgInfo &a = data.avg.find(avg[i].name)->second; out << avg[i].name << ": " << avg[i].value << " (stddev = " << - sqrt(fabs(a.totalSqr - (double)a.parts * avg[i].value * avg[i].value) / ((double)a.parts - 1.)) << ")" << std::endl; + sqrt(std::fabs(a.totalSqr - (double)a.parts * avg[i].value * avg[i].value) / ((double)a.parts - 1.)) << ")" << std::endl; } std::vector time; diff --git a/test/general_test.cpp b/test/general_test.cpp index 1c362da7a..56bdc6ffa 100644 --- a/test/general_test.cpp +++ b/test/general_test.cpp @@ -3,7 +3,6 @@ #include #include #include -#include using namespace std; using namespace fcl; @@ -48,7 +47,7 @@ int main(int argc, char** argv) result.getContacts(contacts); cout << contacts.size() << " contacts found" << endl; - BOOST_FOREACH(Contact& contact, contacts) { + for(const Contact &contact : contacts) { cout << "position: " << contact.pos << endl; } } \ No newline at end of file diff --git a/test/test_fcl_octomap.cpp b/test/test_fcl_octomap.cpp index c31c8e43c..fe53fed75 100644 --- a/test/test_fcl_octomap.cpp +++ b/test/test_fcl_octomap.cpp @@ -631,7 +631,7 @@ void octomap_distance_test(double env_scale, std::size_t env_size, bool use_mesh void generateBoxesFromOctomap(std::vector& boxes, OcTree& tree) { - std::vector > boxes_ = tree.toBoxes(); + std::vector > boxes_ = tree.toBoxes(); for(std::size_t i = 0; i < boxes_.size(); ++i) { @@ -684,7 +684,7 @@ void generateEnvironments(std::vector& env, double env_scale, void generateBoxesFromOctomapMesh(std::vector& boxes, OcTree& tree) { - std::vector > boxes_ = tree.toBoxes(); + std::vector > boxes_ = tree.toBoxes(); for(std::size_t i = 0; i < boxes_.size(); ++i) { diff --git a/test/test_fcl_simple.cpp b/test/test_fcl_simple.cpp index 37688dd59..03839dedf 100644 --- a/test/test_fcl_simple.cpp +++ b/test/test_fcl_simple.cpp @@ -11,8 +11,6 @@ #include "fcl/math/sampling.h" #include "fcl/knn/nearest_neighbors_GNAT.h" -#include - using namespace fcl; static FCL_REAL epsilon = 1e-6; @@ -86,7 +84,7 @@ BOOST_AUTO_TEST_CASE(Vec_nf_test) for(int i = 0; i < 4; ++i) upper[i] = 1; - Vecnf<4> aa(boost::assign::list_of(1)(2).convert_to_container >()); + Vecnf<4> aa(std::vector({1,2})); std::cout << aa << std::endl; SamplerR<4> sampler(lower, upper); From 3f924f62acbd12ae34be03df50d10abe802da95d Mon Sep 17 00:00:00 2001 From: Mark Moll Date: Thu, 24 Mar 2016 19:38:47 -0500 Subject: [PATCH 2/2] use std::abs instead of std::fabs --- src/profile.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/profile.cpp b/src/profile.cpp index eb0b8d24e..7aee359e5 100644 --- a/src/profile.cpp +++ b/src/profile.cpp @@ -222,7 +222,7 @@ void fcl::tools::Profiler::printThreadInfo(std::ostream &out, const PerThread &d { const AvgInfo &a = data.avg.find(avg[i].name)->second; out << avg[i].name << ": " << avg[i].value << " (stddev = " << - sqrt(std::fabs(a.totalSqr - (double)a.parts * avg[i].value * avg[i].value) / ((double)a.parts - 1.)) << ")" << std::endl; + std::sqrt(std::abs(a.totalSqr - (double)a.parts * avg[i].value * avg[i].value) / ((double)a.parts - 1.)) << ")" << std::endl; } std::vector time;