Merge pull request #156 from flexible-collision-library/fix_spatial_h…

…ashing

Fix redundant pair checking of SpatialHashingCollisionManager

.appveyor.yml

@@ -7,10 +7,10 @@ os: Visual Studio 2015
 clone_folder: C:\projects\fcl
 shallow_clone: true
 
-# branches:
-#   only:
-#     - master
-platform: x64
+# build platform, i.e. Win32 (instead of x86), x64, Any CPU. This setting is optional.
+platform:
+  - Win32
+  - x64
 
 environment:
   CTEST_OUTPUT_ON_FAILURE: 1
@@ -23,11 +23,15 @@ configuration:
   - Release
 
 before_build:
+  - cmd: if "%platform%"=="Win32" set CMAKE_GENERATOR_NAME=Visual Studio 14 2015
+  - cmd: if "%platform%"=="x64"   set CMAKE_GENERATOR_NAME=Visual Studio 14 2015 Win64
+  - cmd: if "%platform%"=="Win32" set PROGRAM_FILES_PATH=Program Files (x86)
+  - cmd: if "%platform%"=="x64"   set PROGRAM_FILES_PATH=Program Files
   - cmd: if not exist C:\"Program Files"\libccd\lib\ccd.lib (
             curl -L -o libccd-2.0.tar.gz https://github.com/danfis/libccd/archive/v2.0.tar.gz &&
             cmake -E tar zxf libccd-2.0.tar.gz &&
             cd libccd-2.0 &&
-            cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=%Configuration% . &&
+            cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%Configuration% . &&
             cmake --build . --target install --config %Configuration% &&
             cd ..
          ) else (echo Using cached libccd)
@@ -37,14 +41,14 @@ before_build:
             cd eigen-eigen-dc6cfdf9bcec &&
             mkdir build &&
             cd build &&
-            cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=%Configuration% .. &&
+            cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%Configuration% .. &&
             cmake --build . --target install --config %Configuration% &&
             cd ..\..
          ) else (echo Using cached Eigen3)
   - cmd: set
   - cmd: mkdir build
   - cmd: cd build
-  - cmd: cmake -G "Visual Studio 14 2015 Win64" -DCMAKE_BUILD_TYPE=%Configuration% -DCCD_INCLUDE_DIRS="C:\Program Files\libccd\include" -DCCD_LIBRARY="C:\Program Files\libccd\lib\ccd.lib" -DEIGEN3_INCLUDE_DIR="C:\Program Files\Eigen\include\eigen3" ..
+  - cmd: cmake -G "%CMAKE_GENERATOR_NAME%" -DCMAKE_BUILD_TYPE=%Configuration% -DCCD_INCLUDE_DIRS="C:\%PROGRAM_FILES_PATH%\libccd\include" -DCCD_LIBRARY="C:\%PROGRAM_FILES_PATH%\libccd\lib\ccd.lib" -DEIGEN3_INCLUDE_DIR="C:\%PROGRAM_FILES_PATH%\Eigen\include\eigen3" ..
 
 build:
   project: C:\projects\fcl\build\fcl.sln

CHANGELOG.md

@@ -2,8 +2,10 @@
 
 ### FCL 0.6.0 (2016-XX-XX)
 
-* Switched to Eigen for math operations: [#150](https://github.com/flexible-collision-library/fcl/pull/150), [#96](https://github.com/flexible-collision-library/fcl/issues/96)
 * Added missing copyright headers:  [#149](https://github.com/flexible-collision-library/fcl/pull/149)
+* Enabled build with SSE option by default: [#159](https://github.com/flexible-collision-library/fcl/pull/159)
+* Switched to Eigen for math operations: [#150](https://github.com/flexible-collision-library/fcl/pull/150), [#96](https://github.com/flexible-collision-library/fcl/issues/96)
+* Fixed redundant pair checking of SpatialHashingCollisionManager: [#156](https://github.com/flexible-collision-library/fcl/pull/156)
 * Removed dependency on boost: [#148](https://github.com/flexible-collision-library/fcl/pull/148), [#147](https://github.com/flexible-collision-library/fcl/pull/147), [#146](https://github.com/flexible-collision-library/fcl/pull/146), [#140](https://github.com/flexible-collision-library/fcl/pull/140)
 
 ### FCL 0.5.0 (2016-07-19)

CMakeLists.txt

@@ -36,14 +36,13 @@ else()
 endif()
 
 # Whether to enable SSE
-option(FCL_USE_SSE "Whether FCL should SSE instructions" OFF)
-set(FCL_HAVE_SSE 0)
+option(FCL_USE_SSE "Whether FCL should SSE instructions" ON)
 if(FCL_USE_SSE)
   if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
-    set(FCL_HAVE_SSE 0) #always disable, for now
     add_definitions(-march=native)
+  elseif(MSVC)
+    add_definitions(/arch:SSE2)
   endif()
-  # TODO: do something similar for other compilers
 endif()
 
 # Coveralls support

include/fcl/BV/AABB.h

@@ -118,7 +118,7 @@ class AABB
   /// @brief Center of the AABB
   Vector3<S> center() const;
 
-  /// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points 
+  /// @brief Distance between two AABBs; P and Q, should not be nullptr, return the nearest points 
   S distance(
       const AABB<S>& other, Vector3<S>* P, Vector3<S>* Q) const;
 

include/fcl/BV/BV_node.h

@@ -86,12 +86,12 @@ struct BVNode : public BVNodeBase
   /// @brief Check whether two BVNode collide
   bool overlap(const BVNode& other) const;
 
-  /// @brief Compute the distance between two BVNode. P1 and P2, if not NULL and
+  /// @brief Compute the distance between two BVNode. P1 and P2, if not nullptr and
   /// the underlying BV supports distance, return the nearest points.
   S distance(
       const BVNode& other,
-      Vector3<S>* P1 = NULL,
-      Vector3<S>* P2 = NULL) const;
+      Vector3<S>* P1 = nullptr,
+      Vector3<S>* P2 = nullptr) const;
 
   /// @brief Access the center of the BV
   Vector3<S> getCenter() const;

include/fcl/BV/OBB.h

@@ -112,8 +112,8 @@ class OBB
   const Vector3<S> center() const;
 
   /// @brief Distance between two OBBs, not implemented.
-  S distance(const OBB& other, Vector3<S>* P = NULL,
-                  Vector3<S>* Q = NULL) const;
+  S distance(const OBB& other, Vector3<S>* P = nullptr,
+                  Vector3<S>* Q = nullptr) const;
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
@@ -368,7 +368,7 @@ OBB<S> merge_largedist(const OBB<S>& b1, const OBB<S>& b2)
     vertex_proj[i].noalias() -= b.axis.col(0) * vertex[i].dot(b.axis.col(0));
   }
 
-  getCovariance<S>(vertex_proj, NULL, NULL, NULL, 16, M);
+  getCovariance<S>(vertex_proj, nullptr, nullptr, nullptr, 16, M);
   eigen_old(M, s, E);
 
   int min, mid, max;
@@ -403,7 +403,7 @@ OBB<S> merge_largedist(const OBB<S>& b1, const OBB<S>& b2)
 
   // set obb centers and extensions
   getExtentAndCenter<S>(
-        vertex, NULL, NULL, NULL, 16, b.axis, b.To, b.extent);
+        vertex, nullptr, nullptr, nullptr, 16, b.axis, b.To, b.extent);
 
   return b;
 }

include/fcl/BV/OBBRSS.h

@@ -95,9 +95,9 @@ class OBBRSS
   /// @brief Center of the OBBRSS
   const Vector3<S> center() const;
 
-  /// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points
+  /// @brief Distance between two OBBRSS; P and Q , is not nullptr, returns the nearest points
   S distance(const OBBRSS<S>& other,
-                  Vector3<S>* P = NULL, Vector3<S>* Q = NULL) const;
+                  Vector3<S>* P = nullptr, Vector3<S>* Q = nullptr) const;
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
@@ -126,23 +126,23 @@ bool overlap(
     const OBBRSS<S>& b2);
 
 /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0)
-/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points
+/// and b2 is in indentity; P and Q, is not nullptr, returns the nearest points
 template <typename S, typename DerivedA, typename DerivedB>
 S distance(
     const Eigen::MatrixBase<DerivedA>& R0,
     const Eigen::MatrixBase<DerivedB>& T0,
     const OBBRSS<S>& b1, const OBBRSS<S>& b2,
-    Vector3<S>* P = NULL, Vector3<S>* Q = NULL);
+    Vector3<S>* P = nullptr, Vector3<S>* Q = nullptr);
 
 /// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0)
-/// and b2 is in indentity; P and Q, is not NULL, returns the nearest points
+/// and b2 is in indentity; P and Q, is not nullptr, returns the nearest points
 template <typename S>
 S distance(
     const Transform3<S>& tf,
     const OBBRSS<S>& b1,
     const OBBRSS<S>& b2,
-    Vector3<S>* P = NULL,
-    Vector3<S>* Q = NULL);
+    Vector3<S>* P = nullptr,
+    Vector3<S>* Q = nullptr);
 
 //============================================================================//
 //                                                                            //

include/fcl/BV/RSS.h

@@ -110,10 +110,10 @@ class RSS
   /// @brief The RSS center
   const Vector3<S> center() const;
 
-  /// @brief the distance between two RSS; P and Q, if not NULL, return the nearest points
+  /// @brief the distance between two RSS; P and Q, if not nullptr, return the nearest points
   S distance(const RSS<S>& other,
-                  Vector3<S>* P = NULL,
-                  Vector3<S>* Q = NULL) const;
+                  Vector3<S>* P = nullptr,
+                  Vector3<S>* Q = nullptr) const;
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
@@ -161,8 +161,8 @@ S rectDistance(
     const Vector3<S>& Tab,
     const S a[2],
     const S b[2],
-    Vector3<S>* P = NULL,
-    Vector3<S>* Q = NULL);
+    Vector3<S>* P = nullptr,
+    Vector3<S>* Q = nullptr);
 
 /// @brief Distance between two oriented rectangles; P and Q (optional return
 /// values) are the closest points in the rectangles, both are in the local
@@ -172,8 +172,8 @@ S rectDistance(
     const Transform3<S>& tfab,
     const S a[2],
     const S b[2],
-    Vector3<S>* P = NULL,
-    Vector3<S>* Q = NULL);
+    Vector3<S>* P = nullptr,
+    Vector3<S>* Q = nullptr);
 
 /// @brief distance between two RSS bounding volumes
 /// P and Q (optional return values) are the closest points in the rectangles,
@@ -186,8 +186,8 @@ S distance(
     const Eigen::MatrixBase<DerivedB>& T0,
     const RSS<S>& b1,
     const RSS<S>& b2,
-    Vector3<S>* P = NULL,
-    Vector3<S>* Q = NULL);
+    Vector3<S>* P = nullptr,
+    Vector3<S>* Q = nullptr);
 
 /// @brief distance between two RSS bounding volumes
 /// P and Q (optional return values) are the closest points in the rectangles,
@@ -199,8 +199,8 @@ S distance(
     const Transform3<S>& tf,
     const RSS<S>& b1,
     const RSS<S>& b2,
-    Vector3<S>* P = NULL,
-    Vector3<S>* Q = NULL);
+    Vector3<S>* P = nullptr,
+    Vector3<S>* Q = nullptr);
 
 /// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and
 /// b2 is in identity.
@@ -488,7 +488,7 @@ RSS<S> RSS<S>::operator +(const RSS<S>& other) const
   Matrix3<S> E; // row first eigen-vectors
   Vector3<S> s(0, 0, 0);
 
-  getCovariance<S>(v, NULL, NULL, NULL, 16, M);
+  getCovariance<S>(v, nullptr, nullptr, nullptr, 16, M);
   eigen_old(M, s, E);
 
   int min, mid, max;
@@ -504,7 +504,7 @@ RSS<S> RSS<S>::operator +(const RSS<S>& other) const
   bv.axis.col(2).noalias() = axis.col(0).cross(axis.col(1));
 
   // set rss origin, rectangle size and radius
-  getRadiusAndOriginAndRectangleSize<S>(v, NULL, NULL, NULL, 16, bv.axis, bv.To, bv.l, bv.r);
+  getRadiusAndOriginAndRectangleSize<S>(v, nullptr, nullptr, nullptr, 16, bv.axis, bv.To, bv.l, bv.r);
 
   return bv;
 }

include/fcl/BV/detail/fitter.h

@@ -109,7 +109,7 @@ void fit3(Vector3<S>* ps, OBB<S>& bv)
   bv.axis.col(0).normalize();
   bv.axis.col(1).noalias() = bv.axis.col(2).cross(bv.axis.col(0));
 
-  getExtentAndCenter<S>(ps, NULL, NULL, NULL, 3, bv.axis, bv.To, bv.extent);
+  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, 3, bv.axis, bv.To, bv.extent);
 }
 
 template <typename S>
@@ -128,12 +128,12 @@ void fitn(Vector3<S>* ps, int n, OBB<S>& bv)
   Matrix3<S> E;
   Vector3<S> s = Vector3<S>::Zero(); // three eigen values
 
-  getCovariance<S>(ps, NULL, NULL, NULL, n, M);
+  getCovariance<S>(ps, nullptr, nullptr, nullptr, n, M);
   eigen_old(M, s, E);
   axisFromEigen(E, s, bv.axis);
 
   // set obb centers and extensions
-  getExtentAndCenter<S>(ps, NULL, NULL, NULL, n, bv.axis, bv.To, bv.extent);
+  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, n, bv.axis, bv.To, bv.extent);
 }
 
 } // namespace OBB_fit_functions
@@ -192,7 +192,7 @@ void fit3(Vector3<S>* ps, RSS<S>& bv)
   bv.axis.col(0).noalias() = e[imax].normalized();
   bv.axis.col(1).noalias() = bv.axis.col(2).cross(bv.axis.col(0));
 
-  getRadiusAndOriginAndRectangleSize<S>(ps, NULL, NULL, NULL, 3, bv.axis, bv.To, bv.l, bv.r);
+  getRadiusAndOriginAndRectangleSize<S>(ps, nullptr, nullptr, nullptr, 3, bv.axis, bv.To, bv.l, bv.r);
 }
 
 template <typename S>
@@ -211,12 +211,12 @@ void fitn(Vector3<S>* ps, int n, RSS<S>& bv)
   Matrix3<S> E; // row first eigen-vectors
   Vector3<S> s = Vector3<S>::Zero();
 
-  getCovariance<S>(ps, NULL, NULL, NULL, n, M);
+  getCovariance<S>(ps, nullptr, nullptr, nullptr, n, M);
   eigen_old(M, s, E);
   axisFromEigen(E, s, bv.axis);
 
   // set rss origin, rectangle size and radius
-  getRadiusAndOriginAndRectangleSize<S>(ps, NULL, NULL, NULL, n, bv.axis, bv.To, bv.l, bv.r);
+  getRadiusAndOriginAndRectangleSize<S>(ps, nullptr, nullptr, nullptr, n, bv.axis, bv.To, bv.l, bv.r);
 }
 
 } // namespace RSS_fit_functions
@@ -296,7 +296,7 @@ void fit3(Vector3<S>* ps, kIOS<S>& bv)
   bv.obb.axis.col(0) = e[imax].normalized();
   bv.obb.axis.col(1).noalias() = bv.obb.axis.col(2).cross(bv.obb.axis.col(0));
 
-  getExtentAndCenter<S>(ps, NULL, NULL, NULL, 3, bv.obb.axis, bv.obb.To, bv.obb.extent);
+  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, 3, bv.obb.axis, bv.obb.To, bv.obb.extent);
 
   // compute radius and center
   S r0;
@@ -322,16 +322,16 @@ void fitn(Vector3<S>* ps, int n, kIOS<S>& bv)
   Matrix3<S> E;
   Vector3<S> s = Vector3<S>::Zero(); // three eigen values;
 
-  getCovariance<S>(ps, NULL, NULL, NULL, n, M);
+  getCovariance<S>(ps, nullptr, nullptr, nullptr, n, M);
   eigen_old(M, s, E);
   axisFromEigen(E, s, bv.obb.axis);
 
-  getExtentAndCenter<S>(ps, NULL, NULL, NULL, n, bv.obb.axis, bv.obb.To, bv.obb.extent);
+  getExtentAndCenter<S>(ps, nullptr, nullptr, nullptr, n, bv.obb.axis, bv.obb.To, bv.obb.extent);
 
   // get center and extension
   const Vector3<S>& center = bv.obb.To;
   const Vector3<S>& extent = bv.obb.extent;
-  S r0 = maximumDistance<S>(ps, NULL, NULL, NULL, n, center);
+  S r0 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, center);
 
   // decide the k in kIOS<S>
   if(extent[0] > kIOS<S>::ratio() * extent[2])
@@ -353,8 +353,8 @@ void fitn(Vector3<S>* ps, int n, kIOS<S>& bv)
     bv.spheres[2].o = center + delta;
 
     S r11 = 0, r12 = 0;
-    r11 = maximumDistance<S>(ps, NULL, NULL, NULL, n, bv.spheres[1].o);
-    r12 = maximumDistance<S>(ps, NULL, NULL, NULL, n, bv.spheres[2].o);
+    r11 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[1].o);
+    r12 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[2].o);
     bv.spheres[1].o.noalias() += bv.obb.axis.col(2) * (-r10 + r11);
     bv.spheres[2].o.noalias() += bv.obb.axis.col(2) * (r10 - r12);
 
@@ -370,8 +370,8 @@ void fitn(Vector3<S>* ps, int n, kIOS<S>& bv)
     bv.spheres[4].o = bv.spheres[0].o + delta;
 
     S r21 = 0, r22 = 0;
-    r21 = maximumDistance<S>(ps, NULL, NULL, NULL, n, bv.spheres[3].o);
-    r22 = maximumDistance<S>(ps, NULL, NULL, NULL, n, bv.spheres[4].o);
+    r21 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[3].o);
+    r22 = maximumDistance<S>(ps, nullptr, nullptr, nullptr, n, bv.spheres[4].o);
 
     bv.spheres[3].o.noalias() += bv.obb.axis.col(1) * (-r10 + r21);
     bv.spheres[4].o.noalias() += bv.obb.axis.col(1) * (r10 - r22);

include/fcl/BV/kDOP.h

@@ -132,7 +132,7 @@ class KDOP
   /// @brief The distance between two KDOP<S, N>. Not implemented.
   S distance(
       const KDOP<S, N>& other,
-      Vector3<S>* P = NULL, Vector3<S>* Q = NULL) const;
+      Vector3<S>* P = nullptr, Vector3<S>* Q = nullptr) const;
 
 private:
   /// @brief Origin's distances to N KDOP planes

include/fcl/BV/kIOS.h

@@ -114,7 +114,7 @@ class kIOS
   /// @brief The distance between two kIOS
   S distance(
       const kIOS<S>& other,
-      Vector3<S>* P = NULL, Vector3<S>* Q = NULL) const;
+      Vector3<S>* P = nullptr, Vector3<S>* Q = nullptr) const;
 
   static constexpr S ratio() { return 1.5; }
   static constexpr S invSinA() { return 2; }
@@ -155,8 +155,8 @@ S distance(
     const Eigen::MatrixBase<DerivedB>& T0,
     const kIOS<S>& b1,
     const kIOS<S>& b2,
-    Vector3<S>* P = NULL,
-    Vector3<S>* Q = NULL);
+    Vector3<S>* P = nullptr,
+    Vector3<S>* Q = nullptr);
 
 /// @brief Approximate distance between two kIOS bounding volumes
 /// @todo P and Q is not returned, need implementation
@@ -165,8 +165,8 @@ S distance(
     const Transform3<S>& tf,
     const kIOS<S>& b1,
     const kIOS<S>& b2,
-    Vector3<S>* P = NULL,
-    Vector3<S>* Q = NULL);
+    Vector3<S>* P = nullptr,
+    Vector3<S>* Q = nullptr);
 
 //============================================================================//
 //                                                                            //

include/fcl/BVH/BVH_front.h

@@ -83,7 +83,7 @@ inline BVHFrontNode::BVHFrontNode(int left_, int right_)
 //==============================================================================
 inline void updateFrontList(BVHFrontList* front_list, int b1, int b2)
 {
-  if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
+  if(front_list) front_list->emplace_back(b1, b2);
 }
 
 } // namespace fcl