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

Add support function cached guess in queries and merge query attribute.

parent 3bad0587
...@@ -136,6 +136,47 @@ struct Contact ...@@ -136,6 +136,47 @@ struct Contact
} }
}; };
struct QueryResult;
/// @brief base class for all query requests
struct QueryRequest
{
/// @brief whether enable gjk intial guess
bool enable_cached_gjk_guess;
/// @brief the gjk intial guess set by user
Vec3f cached_gjk_guess;
/// @brief the support function intial guess set by user
support_func_guess_t cached_support_func_guess;
QueryRequest () :
enable_cached_gjk_guess (false),
cached_gjk_guess (1,0,0),
cached_support_func_guess(support_func_guess_t::Zero())
{}
void updateGuess(const QueryResult& result);
};
/// @brief base class for all query results
struct QueryResult
{
/// @brief stores the last GJK ray when relevant.
Vec3f cached_gjk_guess;
/// @brief stores the last support function vertex index, when relevant.
support_func_guess_t cached_support_func_guess;
};
inline void QueryRequest::updateGuess(const QueryResult& result)
{
if (enable_cached_gjk_guess) {
cached_gjk_guess = result.cached_gjk_guess;
cached_support_func_guess = result.cached_support_func_guess;
}
}
struct CollisionResult; struct CollisionResult;
/// @brief flag declaration for specifying required params in CollisionResult /// @brief flag declaration for specifying required params in CollisionResult
...@@ -147,7 +188,7 @@ enum CollisionRequestFlag ...@@ -147,7 +188,7 @@ enum CollisionRequestFlag
}; };
/// @brief request to the collision algorithm /// @brief request to the collision algorithm
struct CollisionRequest struct CollisionRequest : QueryRequest
{ {
/// @brief The maximum number of contacts will return /// @brief The maximum number of contacts will return
size_t num_max_contacts; size_t num_max_contacts;
...@@ -158,12 +199,6 @@ struct CollisionRequest ...@@ -158,12 +199,6 @@ struct CollisionRequest
/// Whether a lower bound on distance is returned when objects are disjoint /// Whether a lower bound on distance is returned when objects are disjoint
bool enable_distance_lower_bound; bool enable_distance_lower_bound;
/// @brief whether enable gjk intial guess
bool enable_cached_gjk_guess;
/// @brief the gjk intial guess set by user
Vec3f cached_gjk_guess;
/// @brief Distance below which objects are considered in collision. /// @brief Distance below which objects are considered in collision.
/// See \ref hpp_fcl_collision_and_distance_lower_bound_computation /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation
FCL_REAL security_margin; FCL_REAL security_margin;
...@@ -187,8 +222,6 @@ struct CollisionRequest ...@@ -187,8 +222,6 @@ struct CollisionRequest
security_margin (0), security_margin (0),
break_distance (1e-3) break_distance (1e-3)
{ {
enable_cached_gjk_guess = false;
cached_gjk_guess = Vec3f(1, 0, 0);
} }
CollisionRequest() : CollisionRequest() :
...@@ -198,23 +231,19 @@ struct CollisionRequest ...@@ -198,23 +231,19 @@ struct CollisionRequest
security_margin (0), security_margin (0),
break_distance (1e-3) break_distance (1e-3)
{ {
enable_cached_gjk_guess = false;
cached_gjk_guess = Vec3f(1, 0, 0);
} }
bool isSatisfied(const CollisionResult& result) const; bool isSatisfied(const CollisionResult& result) const;
}; };
/// @brief collision result /// @brief collision result
struct CollisionResult struct CollisionResult : QueryResult
{ {
private: private:
/// @brief contact information /// @brief contact information
std::vector<Contact> contacts; std::vector<Contact> contacts;
public: public:
Vec3f cached_gjk_guess;
/// Lower bound on distance between objects if they are disjoint. /// Lower bound on distance between objects if they are disjoint.
/// See \ref hpp_fcl_collision_and_distance_lower_bound_computation /// See \ref hpp_fcl_collision_and_distance_lower_bound_computation
/// @note computed only on request (or if it does not add any computational /// @note computed only on request (or if it does not add any computational
...@@ -289,11 +318,10 @@ public: ...@@ -289,11 +318,10 @@ public:
friend void invertResults(CollisionResult& result); friend void invertResults(CollisionResult& result);
}; };
struct DistanceResult; struct DistanceResult;
/// @brief request to the distance computation /// @brief request to the distance computation
struct DistanceRequest struct DistanceRequest : QueryRequest
{ {
/// @brief whether to return the nearest points /// @brief whether to return the nearest points
bool enable_nearest_points; bool enable_nearest_points;
...@@ -328,9 +356,8 @@ struct DistanceRequest ...@@ -328,9 +356,8 @@ struct DistanceRequest
bool isSatisfied(const DistanceResult& result) const; bool isSatisfied(const DistanceResult& result) const;
}; };
/// @brief distance result /// @brief distance result
struct DistanceResult struct DistanceResult : QueryResult
{ {
public: public:
...@@ -456,7 +483,6 @@ public: ...@@ -456,7 +483,6 @@ public:
}; };
inline CollisionRequestFlag operator~(CollisionRequestFlag a) inline CollisionRequestFlag operator~(CollisionRequestFlag a)
{return static_cast<CollisionRequestFlag>(~static_cast<const int>(a));} {return static_cast<CollisionRequestFlag>(~static_cast<const int>(a));}
......
...@@ -72,6 +72,7 @@ typedef boost::uint32_t FCL_UINT32; ...@@ -72,6 +72,7 @@ typedef boost::uint32_t FCL_UINT32;
typedef boost::int32_t FCL_INT32; typedef boost::int32_t FCL_INT32;
typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f; typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f; typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
typedef Eigen::Vector2i support_func_guess_t;
/// @brief Triangle with 3 indices for points /// @brief Triangle with 3 indices for points
class Triangle class Triangle
......
...@@ -61,8 +61,6 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, ...@@ -61,8 +61,6 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized,
/// @note The Minkowski difference is expressed in the frame of the first shape. /// @note The Minkowski difference is expressed in the frame of the first shape.
struct MinkowskiDiff struct MinkowskiDiff
{ {
typedef Eigen::Vector2i hint_t;
/// @brief points to two shapes /// @brief points to two shapes
const ShapeBase* shapes[2]; const ShapeBase* shapes[2];
...@@ -80,7 +78,7 @@ struct MinkowskiDiff ...@@ -80,7 +78,7 @@ struct MinkowskiDiff
typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff, typedef void (*GetSupportFunction) (const MinkowskiDiff& minkowskiDiff,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1, const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
hint_t& hint); support_func_guess_t& hint);
GetSupportFunction getSupportFunc; GetSupportFunction getSupportFunc;
MinkowskiDiff() : getSupportFunc (NULL) {} MinkowskiDiff() : getSupportFunc (NULL) {}
...@@ -106,7 +104,7 @@ struct MinkowskiDiff ...@@ -106,7 +104,7 @@ struct MinkowskiDiff
} }
/// @brief support function for the pair of shapes /// @brief support function for the pair of shapes
inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, hint_t& hint) const inline void support(const Vec3f& d, bool dIsNormalized, Vec3f& supp0, Vec3f& supp1, support_func_guess_t& hint) const
{ {
assert(getSupportFunc != NULL); assert(getSupportFunc != NULL);
getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint); getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint);
...@@ -127,7 +125,6 @@ struct GJK ...@@ -127,7 +125,6 @@ struct GJK
}; };
typedef unsigned char vertex_id_t; typedef unsigned char vertex_id_t;
typedef MinkowskiDiff::hint_t support_hint_t;
struct Simplex struct Simplex
{ {
...@@ -143,7 +140,7 @@ struct GJK ...@@ -143,7 +140,7 @@ struct GJK
MinkowskiDiff const* shape; MinkowskiDiff const* shape;
Vec3f ray; Vec3f ray;
support_hint_t support_hint; support_func_guess_t support_hint;
/// The distance computed by GJK. The possible values are /// The distance computed by GJK. The possible values are
/// - \f$ d = - R - 1 \f$ when a collision is detected and GJK /// - \f$ d = - R - 1 \f$ when a collision is detected and GJK
/// cannot compute penetration informations. /// cannot compute penetration informations.
...@@ -171,11 +168,11 @@ struct GJK ...@@ -171,11 +168,11 @@ struct GJK
/// @brief GJK algorithm, given the initial value guess /// @brief GJK algorithm, given the initial value guess
Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess, Status evaluate(const MinkowskiDiff& shape, const Vec3f& guess,
const support_hint_t& supportHint = support_hint_t::Zero()); const support_func_guess_t& supportHint = support_func_guess_t::Zero());
/// @brief apply the support function along a direction, the result is return in sv /// @brief apply the support function along a direction, the result is return in sv
inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv, inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv,
support_hint_t& hint) const support_func_guess_t& hint) const
{ {
shape->support(d, dIsNormalized, sv.w0, sv.w1, hint); shape->support(d, dIsNormalized, sv.w0, sv.w1, hint);
sv.w.noalias() = sv.w0 - sv.w1; sv.w.noalias() = sv.w0 - sv.w1;
...@@ -238,7 +235,7 @@ private: ...@@ -238,7 +235,7 @@ private:
/// @brief append one vertex to the simplex /// @brief append one vertex to the simplex
inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized, inline void appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized,
support_hint_t& hint); support_func_guess_t& hint);
/// @brief Project origin (0) onto line a-b /// @brief Project origin (0) onto line a-b
bool projectLineOrigin(const Simplex& current, Simplex& next); bool projectLineOrigin(const Simplex& current, Simplex& next);
......
...@@ -52,8 +52,6 @@ namespace fcl ...@@ -52,8 +52,6 @@ namespace fcl
/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet) /// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
struct GJKSolver struct GJKSolver
{ {
typedef details::GJK::support_hint_t support_func_guess_t;
/// @brief intersection checking between two shapes /// @brief intersection checking between two shapes
template<typename S1, typename S2> template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1, bool shapeIntersect(const S1& s1, const Transform3f& tf1,
......
...@@ -85,8 +85,10 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, ...@@ -85,8 +85,10 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
{ {
GJKSolver solver; GJKSolver solver;
solver.enable_cached_guess = request.enable_cached_gjk_guess; solver.enable_cached_guess = request.enable_cached_gjk_guess;
if (solver.enable_cached_guess) if (solver.enable_cached_guess) {
solver.cached_guess = request.cached_gjk_guess; solver.cached_guess = request.cached_gjk_guess;
solver.support_func_cached_guess = request.cached_support_func_guess;
}
const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
std::size_t res; std::size_t res;
...@@ -126,8 +128,10 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, ...@@ -126,8 +128,10 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, &solver, request, result); res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, &solver, request, result);
} }
} }
if (solver.enable_cached_guess) if (solver.enable_cached_guess) {
result.cached_gjk_guess = solver.cached_guess; result.cached_gjk_guess = solver.cached_guess;
result.cached_support_func_guess = solver.support_func_cached_guess;
}
return res; return res;
} }
......
...@@ -61,10 +61,7 @@ bool DistanceRequest::isSatisfied(const DistanceResult& result) const ...@@ -61,10 +61,7 @@ bool DistanceRequest::isSatisfied(const DistanceResult& result) const
enable_distance_lower_bound (enable_distance_lower_bound_), enable_distance_lower_bound (enable_distance_lower_bound_),
security_margin (0), security_margin (0),
break_distance (1e-3) break_distance (1e-3)
{ {}
enable_cached_gjk_guess = false;
cached_gjk_guess = Vec3f(1, 0, 0);
}
} }
} // namespace hpp } // namespace hpp
...@@ -65,6 +65,11 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, ...@@ -65,6 +65,11 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
const DistanceRequest& request, DistanceResult& result) const DistanceRequest& request, DistanceResult& result)
{ {
GJKSolver solver; GJKSolver solver;
solver.enable_cached_guess = request.enable_cached_gjk_guess;
if (solver.enable_cached_guess) {
solver.cached_guess = request.cached_gjk_guess;
solver.support_func_cached_guess = request.cached_support_func_guess;
}
const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable(); const DistanceFunctionMatrix& looktable = getDistanceFunctionLookTable();
...@@ -106,6 +111,10 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, ...@@ -106,6 +111,10 @@ FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1,
res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, &solver, request, result); res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, &solver, request, result);
} }
} }
if (solver.enable_cached_guess) {
result.cached_gjk_guess = solver.cached_guess;
result.cached_support_func_guess = solver.support_func_cached_guess;
}
return res; return res;
} }
......
...@@ -271,7 +271,7 @@ template <typename Shape0, typename Shape1, bool TransformIsIdentity> ...@@ -271,7 +271,7 @@ template <typename Shape0, typename Shape1, bool TransformIsIdentity>
void getSupportTpl (const Shape0* s0, const Shape1* s1, void getSupportTpl (const Shape0* s0, const Shape1* s1,
const Matrix3f& oR1, const Vec3f& ot1, const Matrix3f& oR1, const Vec3f& ot1,
const Vec3f& dir, Vec3f& support0, Vec3f& support1, const Vec3f& dir, Vec3f& support0, Vec3f& support1,
MinkowskiDiff::hint_t& hint) support_func_guess_t& hint)
{ {
getShapeSupport (s0, dir, support0, hint[0]); getShapeSupport (s0, dir, support0, hint[0]);
if (TransformIsIdentity) if (TransformIsIdentity)
...@@ -285,7 +285,7 @@ void getSupportTpl (const Shape0* s0, const Shape1* s1, ...@@ -285,7 +285,7 @@ void getSupportTpl (const Shape0* s0, const Shape1* s1,
template <typename Shape0, typename Shape1, bool TransformIsIdentity> template <typename Shape0, typename Shape1, bool TransformIsIdentity>
void getSupportFuncTpl (const MinkowskiDiff& md, void getSupportFuncTpl (const MinkowskiDiff& md,
const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1, const Vec3f& dir, bool dirIsNormalized, Vec3f& support0, Vec3f& support1,
MinkowskiDiff::hint_t& hint) support_func_guess_t& hint)
{ {
enum { NeedNormalizedDir = enum { NeedNormalizedDir =
bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir bool ( (bool)shape_traits<Shape0>::NeedNormalizedDir
...@@ -512,7 +512,7 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) ...@@ -512,7 +512,7 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1)
} }
GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
const MinkowskiDiff::hint_t& supportHint) const support_func_guess_t& supportHint)
{ {
size_t iterations = 0; size_t iterations = 0;
FCL_REAL alpha = 0; FCL_REAL alpha = 0;
...@@ -635,7 +635,7 @@ inline void GJK::removeVertex(Simplex& simplex) ...@@ -635,7 +635,7 @@ inline void GJK::removeVertex(Simplex& simplex)
free_v[nfree++] = simplex.vertex[--simplex.rank]; free_v[nfree++] = simplex.vertex[--simplex.rank];
} }
inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized, support_hint_t& hint) inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalized, support_func_guess_t& hint)
{ {
simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory simplex.vertex[simplex.rank] = free_v[--nfree]; // set the memory
getSupport (v, isNormalized, *simplex.vertex[simplex.rank++], hint); getSupport (v, isNormalized, *simplex.vertex[simplex.rank++], hint);
...@@ -644,7 +644,7 @@ inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalize ...@@ -644,7 +644,7 @@ inline void GJK::appendVertex(Simplex& simplex, const Vec3f& v, bool isNormalize
bool GJK::encloseOrigin() bool GJK::encloseOrigin()
{ {
Vec3f axis(Vec3f::Zero()); Vec3f axis(Vec3f::Zero());
support_hint_t hint = support_hint_t::Zero(); support_func_guess_t hint = support_func_guess_t::Zero();
switch(simplex->rank) switch(simplex->rank)
{ {
case 1: case 1:
...@@ -1272,7 +1272,7 @@ EPA::SimplexF* EPA::findBest() ...@@ -1272,7 +1272,7 @@ EPA::SimplexF* EPA::findBest()
EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
{ {
GJK::Simplex& simplex = *gjk.getSimplex(); GJK::Simplex& simplex = *gjk.getSimplex();
MinkowskiDiff::hint_t hint (gjk.support_hint); support_func_guess_t hint (gjk.support_hint);
if((simplex.rank > 1) && gjk.encloseOrigin()) if((simplex.rank > 1) && gjk.encloseOrigin())
{ {
while(hull.root) while(hull.root)
......
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