Unverified Commit 87ad1dc9 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #216 from jcarpent/topic/timings

Reduce impact of timings + better throwing messages
parents 8709c3eb 47be80b9
Pipeline #13853 passed with stage
in 45 minutes and 29 seconds
......@@ -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
......
......@@ -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;
}
};
......
......@@ -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
......
......@@ -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;
......
......@@ -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;
......
......@@ -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>
......
......@@ -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
};
}}
......
......@@ -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)
;
}
......
......@@ -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
......@@ -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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment