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

Merge pull request #168 from jmirabel/devel

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