Unverified Commit 1862a016 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #169 from jmirabel/devel

Add collide and distance prototype that update the GJK guess.
parents b5f7f3e6 def58ee8
...@@ -59,6 +59,30 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, ...@@ -59,6 +59,30 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2, const CollisionGeometry* o2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result); const CollisionRequest& request, CollisionResult& result);
/// @copydoc collide(const CollisionObject*, const CollisionObject*, const CollisionRequest&, CollisionResult&)
/// \note this function update the initial guess of \c request if requested.
/// See QueryRequest::updateGuess
inline std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
CollisionRequest& request, CollisionResult& result)
{
std::size_t res = collide(o1, o2, (const CollisionRequest&) request, result);
request.updateGuess (result);
return res;
}
/// @copydoc collide(const CollisionObject*, const CollisionObject*, const CollisionRequest&, CollisionResult&)
/// \note this function update the initial guess of \c request if requested.
/// See QueryRequest::updateGuess
inline std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
CollisionRequest& request, CollisionResult& result)
{
std::size_t res = collide(o1, tf1, o2, tf2,
(const CollisionRequest&) request, result);
request.updateGuess (result);
return res;
}
} }
} // namespace hpp } // namespace hpp
......
...@@ -54,6 +54,30 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const Di ...@@ -54,6 +54,30 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const Di
FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result); const DistanceRequest& request, DistanceResult& result);
/// @copydoc distance(const CollisionObject*, const CollisionObject*, const DistanceRequest&, DistanceResult&)
/// \note this function update the initial guess of \c request if requested.
/// See QueryRequest::updateGuess
inline FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2,
DistanceRequest& request, DistanceResult& result)
{
FCL_REAL res = distance(o1, o2, (const DistanceRequest&) request, result);
request.updateGuess (result);
return res;
}
/// @copydoc distance(const CollisionObject*, const CollisionObject*, const DistanceRequest&, DistanceResult&)
/// \note this function update the initial guess of \c request if requested.
/// See QueryRequest::updateGuess
inline FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
DistanceRequest& request, DistanceResult& result)
{
FCL_REAL res = distance(o1, tf1, o2, tf2,
(const DistanceRequest&) request, result);
request.updateGuess (result);
return res;
}
} }
} // namespace hpp } // namespace hpp
......
...@@ -78,17 +78,26 @@ void exposeCollisionAPI () ...@@ -78,17 +78,26 @@ void exposeCollisionAPI ()
; ;
} }
if(!eigenpy::register_symbolic_link_to_registered_type<QueryRequest>())
{
class_ <QueryRequest> ("QueryRequest",
doxygen::class_doc<QueryRequest>(), no_init)
.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_CLASS_FUNC (QueryRequest, updateGuess)
;
}
if(!eigenpy::register_symbolic_link_to_registered_type<CollisionRequest>()) if(!eigenpy::register_symbolic_link_to_registered_type<CollisionRequest>())
{ {
class_ <CollisionRequest> ("CollisionRequest", class_ <CollisionRequest, bases<QueryRequest> > ("CollisionRequest",
doxygen::class_doc<CollisionRequest>(), no_init) doxygen::class_doc<CollisionRequest>(), no_init)
.def (dv::init<CollisionRequest>()) .def (dv::init<CollisionRequest>())
.def (dv::init<CollisionRequest, const CollisionRequestFlag, size_t>()) .def (dv::init<CollisionRequest, const CollisionRequestFlag, size_t>())
.DEF_RW_CLASS_ATTRIB (CollisionRequest, num_max_contacts ) .DEF_RW_CLASS_ATTRIB (CollisionRequest, num_max_contacts )
.DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_contact ) .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_contact )
.DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_distance_lower_bound) .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_distance_lower_bound)
.DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_cached_gjk_guess )
.DEF_RW_CLASS_ATTRIB (CollisionRequest, cached_gjk_guess )
.DEF_RW_CLASS_ATTRIB (CollisionRequest, security_margin ) .DEF_RW_CLASS_ATTRIB (CollisionRequest, security_margin )
.DEF_RW_CLASS_ATTRIB (CollisionRequest, break_distance ) .DEF_RW_CLASS_ATTRIB (CollisionRequest, break_distance )
; ;
...@@ -119,9 +128,18 @@ void exposeCollisionAPI () ...@@ -119,9 +128,18 @@ void exposeCollisionAPI ()
; ;
} }
if(!eigenpy::register_symbolic_link_to_registered_type<QueryResult>())
{
class_ <QueryResult> ("QueryResult",
doxygen::class_doc<QueryResult>(), no_init)
.DEF_RW_CLASS_ATTRIB (QueryResult, cached_gjk_guess )
.DEF_RW_CLASS_ATTRIB (QueryResult, cached_support_func_guess)
;
}
if(!eigenpy::register_symbolic_link_to_registered_type< CollisionResult >()) if(!eigenpy::register_symbolic_link_to_registered_type< CollisionResult >())
{ {
class_ <CollisionResult> ("CollisionResult", class_ <CollisionResult, bases<QueryResult> > ("CollisionResult",
doxygen::class_doc<CollisionResult>(), no_init) doxygen::class_doc<CollisionResult>(), no_init)
.def (dv::init<CollisionResult>()) .def (dv::init<CollisionResult>())
.DEF_CLASS_FUNC (CollisionResult, isCollision) .DEF_CLASS_FUNC (CollisionResult, isCollision)
...@@ -147,5 +165,5 @@ void exposeCollisionAPI () ...@@ -147,5 +165,5 @@ void exposeCollisionAPI ()
doxygen::def ("collide", static_cast< std::size_t (*)( doxygen::def ("collide", static_cast< std::size_t (*)(
const CollisionGeometry*, const Transform3f&, const CollisionGeometry*, const Transform3f&,
const CollisionGeometry*, const Transform3f&, const CollisionGeometry*, const Transform3f&,
const CollisionRequest&, CollisionResult&) > (&collide)); CollisionRequest&, CollisionResult&) > (&collide));
} }
...@@ -78,7 +78,7 @@ void exposeDistanceAPI () ...@@ -78,7 +78,7 @@ void exposeDistanceAPI ()
{ {
if(!eigenpy::register_symbolic_link_to_registered_type<DistanceRequest>()) if(!eigenpy::register_symbolic_link_to_registered_type<DistanceRequest>())
{ {
class_ <DistanceRequest> ("DistanceRequest", class_ <DistanceRequest, bases<QueryRequest> > ("DistanceRequest",
doxygen::class_doc<DistanceRequest>(), doxygen::class_doc<DistanceRequest>(),
init<optional<bool,FCL_REAL,FCL_REAL> >((arg("self"), init<optional<bool,FCL_REAL,FCL_REAL> >((arg("self"),
arg("enable_nearest_points"), arg("enable_nearest_points"),
...@@ -92,7 +92,7 @@ void exposeDistanceAPI () ...@@ -92,7 +92,7 @@ void exposeDistanceAPI ()
if(!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>()) if(!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>())
{ {
class_ <DistanceResult> ("DistanceResult", class_ <DistanceResult, bases<QueryResult> > ("DistanceResult",
doxygen::class_doc<DistanceResult>(), doxygen::class_doc<DistanceResult>(),
no_init) no_init)
.def (dv::init<DistanceResult>()) .def (dv::init<DistanceResult>())
...@@ -125,5 +125,5 @@ void exposeDistanceAPI () ...@@ -125,5 +125,5 @@ void exposeDistanceAPI ()
doxygen::def ("distance", static_cast< FCL_REAL (*)( doxygen::def ("distance", static_cast< FCL_REAL (*)(
const CollisionGeometry*, const Transform3f&, const CollisionGeometry*, const Transform3f&,
const CollisionGeometry*, const Transform3f&, const CollisionGeometry*, const Transform3f&,
const DistanceRequest&, DistanceResult&) > (&distance)); DistanceRequest&, DistanceResult&) > (&distance));
} }
...@@ -42,8 +42,8 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, int num_points, ...@@ -42,8 +42,8 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, int num_points,
throw std::logic_error ("Qhull failed"); throw std::logic_error ("Qhull failed");
} }
typedef std::size_t index_type;
typedef int size_type; typedef int size_type;
typedef int index_type;
// Map index in pts to index in vertices. -1 means not used // Map index in pts to index in vertices. -1 means not used
std::vector<int> pts_to_vertices (num_points, -1); std::vector<int> pts_to_vertices (num_points, -1);
......
Markdown is supported
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