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,
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
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
......
......@@ -54,6 +54,30 @@ FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const Di
FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
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
......
......@@ -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>())
{
class_ <CollisionRequest> ("CollisionRequest",
class_ <CollisionRequest, bases<QueryRequest> > ("CollisionRequest",
doxygen::class_doc<CollisionRequest>(), no_init)
.def (dv::init<CollisionRequest>())
.def (dv::init<CollisionRequest, const CollisionRequestFlag, size_t>())
.DEF_RW_CLASS_ATTRIB (CollisionRequest, num_max_contacts )
.DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_contact )
.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, break_distance )
;
......@@ -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 >())
{
class_ <CollisionResult> ("CollisionResult",
class_ <CollisionResult, bases<QueryResult> > ("CollisionResult",
doxygen::class_doc<CollisionResult>(), no_init)
.def (dv::init<CollisionResult>())
.DEF_CLASS_FUNC (CollisionResult, isCollision)
......@@ -147,5 +165,5 @@ void exposeCollisionAPI ()
doxygen::def ("collide", static_cast< std::size_t (*)(
const CollisionGeometry*, const Transform3f&,
const CollisionGeometry*, const Transform3f&,
const CollisionRequest&, CollisionResult&) > (&collide));
CollisionRequest&, CollisionResult&) > (&collide));
}
......@@ -78,7 +78,7 @@ void exposeDistanceAPI ()
{
if(!eigenpy::register_symbolic_link_to_registered_type<DistanceRequest>())
{
class_ <DistanceRequest> ("DistanceRequest",
class_ <DistanceRequest, bases<QueryRequest> > ("DistanceRequest",
doxygen::class_doc<DistanceRequest>(),
init<optional<bool,FCL_REAL,FCL_REAL> >((arg("self"),
arg("enable_nearest_points"),
......@@ -92,7 +92,7 @@ void exposeDistanceAPI ()
if(!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>())
{
class_ <DistanceResult> ("DistanceResult",
class_ <DistanceResult, bases<QueryResult> > ("DistanceResult",
doxygen::class_doc<DistanceResult>(),
no_init)
.def (dv::init<DistanceResult>())
......@@ -125,5 +125,5 @@ void exposeDistanceAPI ()
doxygen::def ("distance", static_cast< FCL_REAL (*)(
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,
throw std::logic_error ("Qhull failed");
}
typedef std::size_t index_type;
typedef int size_type;
typedef int index_type;
// Map index in pts to index in vertices. -1 means not used
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