Commit ccd02b91 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add ComputeCollision

parent 14a57cd5
......@@ -42,6 +42,7 @@
#include <hpp/fcl/data_types.h>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision_data.h>
#include <hpp/fcl/collision_func_matrix.h>
namespace hpp
{
......@@ -83,8 +84,60 @@ inline std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
request.updateGuess (result);
return res;
}
}
/// This class reduces the cost of identifying the geometry pair.
/// This is mostly useful for repeated shape-shape queries.
///
/// \code
/// ComputeCollision calc_collision (o1, o2);
/// std::size_t ncontacts = calc_collision(tf1, tf2, request, result);
/// \endcode
class HPP_FCL_DLLAPI ComputeCollision {
public:
ComputeCollision(const CollisionGeometry* o1, const CollisionGeometry* o2);
std::size_t operator()(const Transform3f& tf1, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result)
{
bool cached = request.enable_cached_gjk_guess;
solver.enable_cached_guess = cached;
if (cached) {
solver.cached_guess = request.cached_gjk_guess;
solver.support_func_cached_guess = request.cached_support_func_guess;
}
std::size_t res;
if (swap_geoms) {
res = func(o2, tf2, o1, tf1, &solver, request, result);
invertResults(result);
} else {
res = func (o1, tf1, o2, tf2, &solver, 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)
{
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;
};
} // namespace fcl
} // namespace hpp
#endif
......@@ -179,4 +179,12 @@ void exposeCollisionAPI ()
const CollisionGeometry*, const Transform3f&,
const CollisionGeometry*, const Transform3f&,
CollisionRequest&, CollisionResult&) > (&collide));
class_<ComputeCollision> ("ComputeCollision",
doxygen::class_doc<ComputeCollision>(), no_init)
.def (dv::init<ComputeCollision, const CollisionGeometry*, const CollisionGeometry*>())
.def ("__call__", static_cast< std::size_t (ComputeCollision::*)(
const Transform3f&, const Transform3f&,
CollisionRequest&, CollisionResult&) > (&ComputeCollision::operator()));
}
......@@ -56,17 +56,11 @@ CollisionFunctionMatrix& getCollisionFunctionLookTable()
// reorder collision results in the order the call has been made.
void invertResults(CollisionResult& result)
{
const CollisionGeometry* otmp;
int btmp;
for(std::vector<Contact>::iterator it = result.contacts.begin();
it != result.contacts.end(); ++it)
{
otmp = it->o1;
it->o1 = it->o2;
it->o2 = otmp;
btmp = it->b1;
it->b1 = it->b2;
it->b2 = btmp;
std::swap(it->o1, it->o2);
std::swap(it->b1, it->b2);
}
}
......@@ -136,7 +130,32 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
return res;
}
}
ComputeCollision::ComputeCollision(const CollisionGeometry* o1,
const CollisionGeometry* o2)
: o1(o1), o2(o2)
{
const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
OBJECT_TYPE object_type1 = o1->getObjectType();
NODE_TYPE node_type1 = o1->getNodeType();
OBJECT_TYPE object_type2 = o2->getObjectType();
NODE_TYPE node_type2 = o2->getNodeType();
swap_geoms = object_type1 == OT_GEOM && object_type2 == OT_BVH;
if( ( swap_geoms && !looktable.collision_matrix[node_type2][node_type1])
|| (!swap_geoms && !looktable.collision_matrix[node_type1][node_type2]))
{
std::ostringstream oss;
oss << "Warning: collision function between node type " << node_type1 <<
" and node type " << node_type2 << " is not supported";
throw std::invalid_argument(oss.str());
}
if (swap_geoms)
func = looktable.collision_matrix[node_type2][node_type1];
else
func = looktable.collision_matrix[node_type1][node_type2];
}
} // 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