Merge pull request #108 from sherm1/get-rid-of-boost-dynamic_bitset

Removes dependence on boost::dynamic_bitset and fixes test failure on Windows

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 <boost/dynamic_bitset.hpp>
 #include "fcl/data_types.h"
 #include "fcl/BV/AABB.h"
 
+#include <bitset>
+
 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<N>` where
+/// N is the length of the code and must be a multiple of 3.
 template<typename T>
 struct morton_functor {};
 
@@ -119,7 +124,7 @@ struct morton_functor<FCL_UINT32>
   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<FCL_UINT64>
   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<boost::dynamic_bitset<> >
+
+/// @brief Functor to compute N bit morton code for a given AABB
+/// N must be a multiple of 3.
+template<size_t N>
+struct morton_functor<std::bitset<N>> 
 {
-  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<N> 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<N> 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; }
 };
 
 }

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

src/broadphase/broadphase_bruteforce.cpp

@@ -37,6 +37,7 @@
 
 #include "fcl/broadphase/broadphase_bruteforce.h"
 #include <limits>
+#include <iterator>
 
 namespace fcl
 {

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<boost::dynamic_bitset<> > F1(bbox, 10);
-  morton_functor<boost::dynamic_bitset<> > F2(bbox, 20);
-  morton_functor<FCL_UINT64> F3(bbox);
-  morton_functor<FCL_UINT32> F4(bbox);
+  morton_functor<std::bitset<30>> F1(bbox);
+  morton_functor<std::bitset<60>> F2(bbox);
+  morton_functor<FCL_UINT64> F3(bbox); // 60 bits
+  morton_functor<FCL_UINT32> 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));
 }