Merge pull request #216 from jcarpent/topic/timings

Reduce impact of timings + better throwing messages

include/hpp/fcl/collision.h

@@ -111,24 +111,23 @@ public:
     solver.distance_upper_bound = request.distance_upper_bound;
 
     std::size_t res;
+    if(request.enable_timings)
     {
       Timer timer;
-      if (swap_geoms) {
-        res = func(o2, tf2, o1, tf1, &solver, request, result);
-        result.swapObjects();
-      } else {
-        res = func (o1, tf1, o2, tf2, &solver, request, result);
-      }
+      res = run(tf1, tf2, request, result);
       result.timings = timer.elapsed();
     }
+    else
+      res = run(tf1, tf2, request, result);
 
     if (cached) {
       result.cached_gjk_guess = solver.cached_guess;
       result.cached_support_func_guess = solver.support_func_cached_guess;
     }
+    
     return res;
   }
-
+  
   inline std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
                                 CollisionRequest& request, CollisionResult& result) const
   {
@@ -136,15 +135,21 @@ public:
     request.updateGuess (result);
     return res;
   }
-
-private:
+  
+  virtual ~ComputeCollision() {};
+  
+protected:
   CollisionGeometry const *o1, *o2;
   GJKSolver solver;
 
   CollisionFunctionMatrix::CollisionFunc func;
   bool swap_geoms;
+
+  virtual std::size_t run(const Transform3f& tf1, const Transform3f& tf2,
+                          const CollisionRequest& request, CollisionResult& result) const;
 };
 
+
 } // namespace fcl
 } // namespace hpp
 

include/hpp/fcl/collision_data.h

@@ -145,11 +145,15 @@ struct HPP_FCL_DLLAPI QueryRequest
 
   /// @brief the support function intial guess set by user
   support_func_guess_t cached_support_func_guess;
+  
+  /// @brief enable timings when performing collision/distance request
+  bool enable_timings;
 
   QueryRequest () :
     enable_cached_gjk_guess (false),
     cached_gjk_guess (1,0,0),
-    cached_support_func_guess(support_func_guess_t::Zero())
+    cached_support_func_guess(support_func_guess_t::Zero()),
+    enable_timings(false)
   {}
 
   void updateGuess(const QueryResult& result);
@@ -159,7 +163,8 @@ struct HPP_FCL_DLLAPI QueryRequest
   {
     return enable_cached_gjk_guess == other.enable_cached_gjk_guess
       && cached_gjk_guess == other.cached_gjk_guess
-      && cached_support_func_guess == other.cached_support_func_guess;
+      && cached_support_func_guess == other.cached_support_func_guess
+      && enable_timings == other.enable_timings;
   }
 };
 

include/hpp/fcl/distance.h

@@ -104,19 +104,14 @@ public:
     }
 
     FCL_REAL res;
+    if(request.enable_timings)
     {
       Timer timer;
-      if (swap_geoms) {
-        res = func(o2, tf2, o1, tf1, &solver, request, result);
-        if (request.enable_nearest_points) {
-          std::swap(result.o1, result.o2);
-          result.nearest_points[0].swap(result.nearest_points[1]);
-        }
-      } else {
-        res = func (o1, tf1, o2, tf2, &solver, request, result);
-      }
+      res = run(tf1, tf2, request, result);
       result.timings = timer.elapsed();
     }
+    else
+      res = run(tf1, tf2, request, result);
 
     if (cached) {
       result.cached_gjk_guess = solver.cached_guess;
@@ -132,15 +127,22 @@ public:
     request.updateGuess (result);
     return res;
   }
+  
+  virtual ~ComputeDistance() {};
 
-private:
+protected:
   CollisionGeometry const *o1, *o2;
   GJKSolver solver;
 
   DistanceFunctionMatrix::DistanceFunc func;
   bool swap_geoms;
+  
+  virtual FCL_REAL run(const Transform3f& tf1, const Transform3f& tf2,
+                       const DistanceRequest& request, DistanceResult& result) const;
 };
 
+
+
 } // namespace fcl
 } // namespace hpp
 

include/hpp/fcl/fwd.hh

@@ -38,9 +38,30 @@
 #define HPP_FCL_FWD_HH
 
 #include <boost/shared_ptr.hpp>
+#include <sstream>
 
 #include <hpp/fcl/config.hh>
 
+#if _WIN32
+  #define HPP_FCL_PRETTY_FUNCTION __FUNCSIG__
+#else
+  #define HPP_FCL_PRETTY_FUNCTION __PRETTY_FUNCTION__
+#endif
+
+#define HPP_FCL_THROW_PRETTY(message,exception) \
+{ \
+std::stringstream ss; \
+ss << "From file: " << __FILE__ << "\n"; \
+ss << "in function: " << HPP_FCL_PRETTY_FUNCTION << "\n"; \
+ss << "at line: " << __LINE__ << "\n"; \
+ss << "message: " << message << "\n"; \
+throw exception(ss.str()); \
+}
+
+#if (__cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1600))
+  #define HPP_FCL_WITH_CXX11_SUPPORT
+#endif
+
 namespace hpp {
 namespace fcl {
   using boost::shared_ptr;

include/hpp/fcl/internal/traversal_node_setup.h

@@ -51,6 +51,7 @@
 
 #include <hpp/fcl/BVH/BVH_utility.h>
 
+
 namespace hpp
 {
 namespace fcl
@@ -308,7 +309,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+    HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
 
   if(!tf1.isIdentity())
   {
@@ -352,7 +353,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, 0>& node,
                 CollisionResult& result)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+    HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
 
   node.model1 = &model1;
   node.tf1 = tf1;
@@ -381,7 +382,7 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S>& node,
                                                        CollisionResult& result)
 {
   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+    HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
 
   node.model1 = &model1;
   node.tf1 = tf1;
@@ -410,8 +411,10 @@ bool initialize(MeshCollisionTraversalNode<BV, RelativeTransformationIsIdentity>
                 CollisionResult& result,
                 bool use_refit = false, bool refit_bottomup = false)
 {
-  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model1 and model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+  if(model1.getModelType() != BVH_MODEL_TRIANGLES)
+    HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
+  if(model2.getModelType() != BVH_MODEL_TRIANGLES)
+    HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
 
   if(!tf1.isIdentity())
   {
@@ -469,8 +472,10 @@ bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
                 const BVHModel<BV>& model2, const Transform3f& tf2,
                 CollisionResult& result)
 {
-  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model1 and model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+  if(model1.getModelType() != BVH_MODEL_TRIANGLES)
+    HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
+  if(model2.getModelType() != BVH_MODEL_TRIANGLES)
+    HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
 
   node.vertices1 = model1.vertices;
   node.vertices2 = model2.vertices;
@@ -485,8 +490,8 @@ bool initialize(MeshCollisionTraversalNode<BV, 0>& node,
 
   node.result = &result;
 
-  node.RT.R = tf1.getRotation().transpose() * tf2.getRotation();
-  node.RT.T = tf1.getRotation().transpose() * (tf2.getTranslation() - tf1.getTranslation());
+  node.RT.R.noalias() = tf1.getRotation().transpose() * tf2.getRotation();
+  node.RT.T.noalias() = tf1.getRotation().transpose() * (tf2.getTranslation() - tf1.getTranslation());
 
   return true;
 }
@@ -521,8 +526,10 @@ bool initialize(MeshDistanceTraversalNode<BV, RelativeTransformationIsIdentity>&
                 DistanceResult& result,
                 bool use_refit = false, bool refit_bottomup = false)
 {
-  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model1 and model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+  if(model1.getModelType() != BVH_MODEL_TRIANGLES)
+    HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
+  if(model2.getModelType() != BVH_MODEL_TRIANGLES)
+    HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
 
   if(!tf1.isIdentity())
   {
@@ -583,8 +590,10 @@ bool initialize(MeshDistanceTraversalNode<BV, 0>& node,
                 const DistanceRequest& request,
                 DistanceResult& result)
 {
-  if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model1 and model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+  if(model1.getModelType() != BVH_MODEL_TRIANGLES)
+    HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
+  if(model2.getModelType() != BVH_MODEL_TRIANGLES)
+    HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
 
   node.request = request;
   node.result = &result;
@@ -618,7 +627,7 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S>& node,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+    HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
 
   if(!tf1.isIdentity())
   {
@@ -665,7 +674,7 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV>& node,
                 bool use_refit = false, bool refit_bottomup = false)
 {
   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+    HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
   
   if(!tf2.isIdentity())
   {
@@ -714,7 +723,7 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S>& node,
                                                       DistanceResult& result)
 {
   if(model1.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
+    HPP_FCL_THROW_PRETTY("model1 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
 
   node.request = request;
   node.result = &result;
@@ -783,8 +792,8 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S>& node,
                                                       DistanceResult& result)
 {
   if(model2.getModelType() != BVH_MODEL_TRIANGLES)
-    throw std::invalid_argument("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.");
-
+    HPP_FCL_THROW_PRETTY("model2 should be of type BVHModelType::BVH_MODEL_TRIANGLES.",std::invalid_argument)
+    
   node.request = request;
   node.result = &result;
 

include/hpp/fcl/serialization/collision_data.h

@@ -49,6 +49,7 @@ namespace boost
       ar & make_nvp("enable_cached_gjk_guess",query_request.enable_cached_gjk_guess);
       ar & make_nvp("cached_gjk_guess",query_request.cached_gjk_guess);
       ar & make_nvp("cached_support_func_guess",query_request.cached_support_func_guess);
+      ar & make_nvp("enable_timings",query_request.enable_timings);
     }
   
     template <class Archive>

include/hpp/fcl/timings.h

@@ -5,17 +5,19 @@
 #ifndef HPP_FCL_TIMINGS_FWD_H
 #define HPP_FCL_TIMINGS_FWD_H
 
-#include <boost/chrono.hpp>
-
 #include "hpp/fcl/fwd.hh"
 
+#ifdef HPP_FCL_WITH_CXX11_SUPPORT
+  #include <chrono>
+#endif
+
 namespace hpp { namespace fcl {
 
   struct CPUTimes
   {
-    FCL_REAL wall;
-    FCL_REAL user;
-    FCL_REAL system;
+    double wall;
+    double user;
+    double system;
     
     CPUTimes()
     : wall(0)
@@ -30,29 +32,15 @@ namespace hpp { namespace fcl {
     
   };
 
-  namespace internal
-  {
-    inline void get_cpu_times(CPUTimes & current)
-    {
-      using namespace boost::chrono;
-      
-      process_real_cpu_clock::time_point wall = process_real_cpu_clock::now();
-      process_user_cpu_clock::time_point user = process_user_cpu_clock::now();
-      process_system_cpu_clock::time_point system = process_system_cpu_clock::now();
-      
-      current.wall = time_point_cast<nanoseconds>(wall).time_since_epoch().count()*1e-3;
-      current.user = time_point_cast<nanoseconds>(user).time_since_epoch().count()*1e-3;
-      current.system = time_point_cast<nanoseconds>(system).time_since_epoch().count()*1e-3;
-    }
-  }
-
   ///
-  /// @brief This class mimics the way "boost/timer/timer.hpp" operates while using moder boost::chrono library.
+  /// @brief This class mimics the way "boost/timer/timer.hpp" operates while using the modern std::chrono library.
+  ///        Importantly, this class will only have an effect for C++11 and more.
   ///
-  struct Timer
+  struct HPP_FCL_DLLAPI Timer
   {
     
     Timer()
+    : m_is_stopped(true)
     {
       start();
     }
@@ -62,18 +50,26 @@ namespace hpp { namespace fcl {
       if(m_is_stopped)
         return m_times;
       
-      CPUTimes current;
-      internal::get_cpu_times(current);
-      current.wall -= m_times.wall;
-      current.user -= m_times.user;
-      current.system -= m_times.system;
+      
+      CPUTimes current(m_times);
+#ifdef HPP_FCL_WITH_CXX11_SUPPORT
+      std::chrono::time_point<std::chrono::steady_clock> current_clock = std::chrono::steady_clock::now();
+      current.user += std::chrono::duration_cast<std::chrono::nanoseconds>(current_clock - m_start).count()*1e-3;
+#endif
       return current;
     }
     
     void start()
     {
-      m_is_stopped = false;
-      internal::get_cpu_times(m_times);
+      if(m_is_stopped)
+      {
+        m_is_stopped = false;
+        m_times.clear();
+        
+#ifdef HPP_FCL_WITH_CXX11_SUPPORT
+        m_start = std::chrono::steady_clock::now();
+#endif
+      }
     }
     
     void stop()
@@ -82,24 +78,19 @@ namespace hpp { namespace fcl {
         return;
       m_is_stopped = true;
       
-      CPUTimes current;
-      internal::get_cpu_times(current);
-      m_times.wall = (current.wall - m_times.wall);
-      m_times.user = (current.user - m_times.user);
-      m_times.system = (current.system - m_times.system);
+#ifdef HPP_FCL_WITH_CXX11_SUPPORT
+      m_end = std::chrono::steady_clock::now();
+      m_times.user += std::chrono::duration_cast<std::chrono::nanoseconds>(m_end - m_start).count()*1e-3;
+#endif
       
     }
     
     void resume()
     {
+#ifdef HPP_FCL_WITH_CXX11_SUPPORT
       if(m_is_stopped)
-      {
-        CPUTimes current(m_times);
-        start();
-        m_times.wall   -= current.wall;
-        m_times.user   -= current.user;
-        m_times.system -= current.system;
-      }
+        m_start = std::chrono::steady_clock::now();
+#endif
     }
     
     bool is_stopped() const
@@ -111,6 +102,10 @@ namespace hpp { namespace fcl {
     
     CPUTimes m_times;
     bool m_is_stopped;
+    
+#ifdef HPP_FCL_WITH_CXX11_SUPPORT
+    std::chrono::time_point<std::chrono::steady_clock> m_start, m_end;
+#endif
   };
 
 }}

python/collision.cc

@@ -98,6 +98,7 @@ void exposeCollisionAPI ()
       .DEF_RW_CLASS_ATTRIB (QueryRequest, enable_cached_gjk_guess    )
       .DEF_RW_CLASS_ATTRIB (QueryRequest, cached_gjk_guess           )
       .DEF_RW_CLASS_ATTRIB (QueryRequest, cached_support_func_guess  )
+      .DEF_RW_CLASS_ATTRIB (QueryRequest, enable_timings  )
       .DEF_CLASS_FUNC (QueryRequest, updateGuess)
       ;
   }

src/collision.cpp

@@ -158,5 +158,18 @@ ComputeCollision::ComputeCollision(const CollisionGeometry* o1,
     func = looktable.collision_matrix[node_type1][node_type2];
 }
 
+std::size_t ComputeCollision::run(const Transform3f& tf1, const Transform3f& tf2,
+       const CollisionRequest& request, CollisionResult& result) const
+{
+  std::size_t res;
+  if (swap_geoms) {
+    res = func(o2, tf2, o1, tf1, &solver, request, result);
+    result.swapObjects();
+  } else {
+    res = func (o1, tf1, o2, tf2, &solver, request, result);
+  }
+  return res;
+}
+
 } // namespace fcl
 } // namespace hpp

src/distance.cpp

@@ -146,5 +146,23 @@ ComputeDistance::ComputeDistance(const CollisionGeometry* o1,
     func = looktable.distance_matrix[node_type1][node_type2];
 }
 
+FCL_REAL ComputeDistance::run(const Transform3f& tf1, const Transform3f& tf2,
+                              const DistanceRequest& request, DistanceResult& result) const
+{
+  FCL_REAL res;
+  
+  if (swap_geoms) {
+    res = func(o2, tf2, o1, tf1, &solver, request, result);
+    if (request.enable_nearest_points) {
+      std::swap(result.o1, result.o2);
+      result.nearest_points[0].swap(result.nearest_points[1]);
+    }
+  } else {
+    res = func (o1, tf1, o2, tf2, &solver, request, result);
+  }
+  
+  return res;
+}
+
 } // namespace fcl
 } // namespace hpp