From 5e1ebb86db97430b6015ae63800d783249e8c3f2 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Sat, 26 Mar 2016 13:15:28 -0700 Subject: [PATCH 1/3] Removes dependence on boost::dynamic_bitset and fixes #107 (test failure on Windows). --- include/fcl/broadphase/morton.h | 49 ++++++++++++++---------- src/broadphase/broadphase_bruteforce.cpp | 1 + test/test_fcl_math.cpp | 17 ++++---- 3 files changed, 37 insertions(+), 30 deletions(-) diff --git a/include/fcl/broadphase/morton.h b/include/fcl/broadphase/morton.h index 11b91ec67..1d26f28b7 100644 --- a/include/fcl/broadphase/morton.h +++ b/include/fcl/broadphase/morton.h @@ -3,6 +3,7 @@ * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2016, Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -38,10 +39,11 @@ #ifndef FCL_MORTON_H #define FCL_MORTON_H -#include #include "fcl/data_types.h" #include "fcl/BV/AABB.h" +#include + namespace fcl { @@ -93,6 +95,9 @@ static inline FCL_UINT64 morton_code60(FCL_UINT32 x, FCL_UINT32 y, FCL_UINT32 z) /// @brief Functor to compute the morton code for a given AABB +/// This is specialized for 32- and 64-bit unsigned integers giving +/// a 30- or 60-bit code, respectively, and for `std::bitset` where +/// N is the length of the code and must be a multiple of 3. template struct morton_functor {}; @@ -119,7 +124,7 @@ struct morton_functor const Vec3f base; const Vec3f inv; - size_t bits() const { return 30; } + static constexpr size_t bits() { return 30; } }; @@ -145,50 +150,52 @@ struct morton_functor const Vec3f base; const Vec3f inv; - size_t bits() const { return 60; } + static constexpr size_t bits() { return 60; } }; -/// @brief Functor to compute n bit morton code for a given AABB -template<> -struct morton_functor > + +/// @brief Functor to compute N bit morton code for a given AABB +/// N must be a multiple of 3. +template +struct morton_functor> { - morton_functor(const AABB& bbox, size_t bit_num_) : base(bbox.min_), - inv(1.0 / (bbox.max_[0] - bbox.min_[0]), - 1.0 / (bbox.max_[1] - bbox.min_[1]), - 1.0 / (bbox.max_[2] - bbox.min_[2])), - bit_num(bit_num_) + static_assert(N%3==0, "Number of bits must be a multiple of 3"); + + morton_functor(const AABB& bbox) : base(bbox.min_), + inv(1.0 / (bbox.max_[0] - bbox.min_[0]), + 1.0 / (bbox.max_[1] - bbox.min_[1]), + 1.0 / (bbox.max_[2] - bbox.min_[2])) {} - boost::dynamic_bitset<> operator() (const Vec3f& point) const + std::bitset operator() (const Vec3f& point) const { FCL_REAL x = (point[0] - base[0]) * inv[0]; FCL_REAL y = (point[1] - base[1]) * inv[1]; FCL_REAL z = (point[2] - base[2]) * inv[2]; - int start_bit = bit_num * 3 - 1; - boost::dynamic_bitset<> bits(bit_num * 3); + int start_bit = bits() - 1; + std::bitset bset; x *= 2; y *= 2; z *= 2; - for(size_t i = 0; i < bit_num; ++i) + for(size_t i = 0; i < bits()/3; ++i) { - bits[start_bit--] = ((z < 1) ? 0 : 1); - bits[start_bit--] = ((y < 1) ? 0 : 1); - bits[start_bit--] = ((x < 1) ? 0 : 1); + bset[start_bit--] = ((z < 1) ? 0 : 1); + bset[start_bit--] = ((y < 1) ? 0 : 1); + bset[start_bit--] = ((x < 1) ? 0 : 1); x = ((x >= 1) ? 2*(x-1) : 2*x); y = ((y >= 1) ? 2*(y-1) : 2*y); z = ((z >= 1) ? 2*(z-1) : 2*z); } - return bits; + return bset; } const Vec3f base; const Vec3f inv; - const size_t bit_num; - size_t bits() const { return bit_num * 3; } + static constexpr size_t bits() { return N; } }; } diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index e6e41fc8c..b18bc9b90 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -37,6 +37,7 @@ #include "fcl/broadphase/broadphase_bruteforce.h" #include +#include namespace fcl { diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp index 9270213ea..ccff3928d 100644 --- a/test/test_fcl_math.cpp +++ b/test/test_fcl_math.cpp @@ -3,6 +3,7 @@ * * Copyright (c) 2011-2014, Willow Garage, Inc. * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2016, Toyota Research Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -629,15 +630,13 @@ BOOST_AUTO_TEST_CASE(vec_test_sse_vec64_consistent) BOOST_AUTO_TEST_CASE(morton) { AABB bbox(Vec3f(0, 0, 0), Vec3f(1000, 1000, 1000)); - morton_functor > F1(bbox, 10); - morton_functor > F2(bbox, 20); - morton_functor F3(bbox); - morton_functor F4(bbox); + morton_functor> F1(bbox); + morton_functor> F2(bbox); + morton_functor F3(bbox); // 60 bits + morton_functor F4(bbox); // 30 bits Vec3f p(254, 873, 674); - std::cout << std::hex << F1(p).to_ulong() << std::endl; - std::cout << std::hex << F2(p).to_ulong() << std::endl; - std::cout << std::hex << F3(p) << std::endl; - std::cout << std::hex << F4(p) << std::endl; - + + BOOST_CHECK(F1(p).to_ulong() == F4(p)); + BOOST_CHECK(F2(p).to_ullong() == F3(p)); } From 5d07547f36ab3e468ba6e33e71457d083a18c991 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Sat, 26 Mar 2016 17:19:18 -0700 Subject: [PATCH 2/3] bitset template argument is a size_t --- include/fcl/broadphase/morton.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/fcl/broadphase/morton.h b/include/fcl/broadphase/morton.h index 1d26f28b7..1d575dba7 100644 --- a/include/fcl/broadphase/morton.h +++ b/include/fcl/broadphase/morton.h @@ -156,7 +156,7 @@ struct morton_functor /// @brief Functor to compute N bit morton code for a given AABB /// N must be a multiple of 3. -template +template struct morton_functor> { static_assert(N%3==0, "Number of bits must be a multiple of 3"); From 33afc0f4e0a497ef65f14ee858ac04ee5fe8b314 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Sat, 26 Mar 2016 17:44:52 -0700 Subject: [PATCH 3/3] Fix specified-size int types. Shouldn't be "fast" because those can be larger than the specified sizes. (Also int/unsigned were backwards.) --- include/fcl/data_types.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/fcl/data_types.h b/include/fcl/data_types.h index 64181944b..8deb98f02 100644 --- a/include/fcl/data_types.h +++ b/include/fcl/data_types.h @@ -45,10 +45,10 @@ namespace fcl { typedef double FCL_REAL; -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; +typedef std::int64_t FCL_INT64; +typedef std::uint64_t FCL_UINT64; +typedef std::int32_t FCL_INT32; +typedef std::uint32_t FCL_UINT32; /// @brief Triangle with 3 indices for points class Triangle