Verified Commit 1479ff00 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo: handle enable_timings feature

parent 02097412
......@@ -111,40 +111,42 @@ 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
std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
CollisionRequest& request, CollisionResult& result) const
{
std::size_t res = operator()(tf1, tf2, (const CollisionRequest&) request, result);
request.updateGuess (result);
return res;
}
private:
CollisionGeometry const *o1, *o2;
GJKSolver solver;
CollisionFunctionMatrix::CollisionFunc func;
bool swap_geoms;
std::size_t run(const Transform3f& tf1, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result) const;
};
} // namespace fcl
} // namespace hpp
......
......@@ -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;
......@@ -139,8 +134,13 @@ private:
DistanceFunctionMatrix::DistanceFunc func;
bool swap_geoms;
FCL_REAL run(const Transform3f& tf1, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result) const;
};
} // namespace fcl
} // namespace hpp
......
......@@ -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