Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Humanoid Path Planner
hpp-fcl
Commits
ada708b6
Commit
ada708b6
authored
May 05, 2020
by
Joseph Mirabel
Browse files
Add support function cached guess in queries and merge query attribute.
parent
3bad0587
Changes
8
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/collision_data.h
View file @
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
));}
...
...
include/hpp/fcl/data_types.h
View file @
ada708b6
...
...
@@ -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
...
...
include/hpp/fcl/narrowphase/gjk.h
View file @
ada708b6
...
...
@@ -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
);
...
...
include/hpp/fcl/narrowphase/narrowphase.h
View file @
ada708b6
...
...
@@ -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
,
...
...
src/collision.cpp
View file @
ada708b6
...
...
@@ -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
;
}
...
...
src/collision_data.cpp
View file @
ada708b6
...
...
@@ -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
src/distance.cpp
View file @
ada708b6
...
...
@@ -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
;
}
...
...
src/narrowphase/gjk.cpp
View file @
ada708b6
...
...
@@ -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
)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment