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
Guilhem Saurel
hpp-fcl
Commits
35d3109b
Commit
35d3109b
authored
May 04, 2020
by
Joseph Mirabel
Browse files
Add hint to the support function
This allows a near constant support function for class Convex.
parent
7eb86853
Changes
4
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/gjk.h
View file @
35d3109b
...
...
@@ -50,7 +50,8 @@ namespace details
{
/// @brief the support function for shape
Vec3f
getSupport
(
const
ShapeBase
*
shape
,
const
Vec3f
&
dir
,
bool
dirIsNormalized
);
/// \param hint use to initialize the search when shape is a ConvexBase object.
Vec3f
getSupport
(
const
ShapeBase
*
shape
,
const
Vec3f
&
dir
,
bool
dirIsNormalized
,
int
&
hint
);
/// @brief Minkowski difference class of two shapes
///
...
...
@@ -60,6 +61,8 @@ 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
];
...
...
@@ -76,7 +79,8 @@ struct MinkowskiDiff
Eigen
::
Array
<
FCL_REAL
,
1
,
2
>
inflation
;
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
);
GetSupportFunction
getSupportFunc
;
MinkowskiDiff
()
:
getSupportFunc
(
NULL
)
{}
...
...
@@ -90,22 +94,22 @@ struct MinkowskiDiff
const
Transform3f
&
tf0
,
const
Transform3f
&
tf1
);
/// @brief support function for shape0
inline
Vec3f
support0
(
const
Vec3f
&
d
,
bool
dIsNormalized
)
const
inline
Vec3f
support0
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
int
&
hint
)
const
{
return
getSupport
(
shapes
[
0
],
d
,
dIsNormalized
);
return
getSupport
(
shapes
[
0
],
d
,
dIsNormalized
,
hint
);
}
/// @brief support function for shape1
inline
Vec3f
support1
(
const
Vec3f
&
d
,
bool
dIsNormalized
)
const
inline
Vec3f
support1
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
int
&
hint
)
const
{
return
oR1
*
getSupport
(
shapes
[
1
],
oR1
.
transpose
()
*
d
,
dIsNormalized
)
+
ot1
;
return
oR1
*
getSupport
(
shapes
[
1
],
oR1
.
transpose
()
*
d
,
dIsNormalized
,
hint
)
+
ot1
;
}
/// @brief support function for the pair of shapes
inline
void
support
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
Vec3f
&
supp0
,
Vec3f
&
supp1
)
const
inline
void
support
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
Vec3f
&
supp0
,
Vec3f
&
supp1
,
hint_t
&
hint
)
const
{
assert
(
getSupportFunc
!=
NULL
);
getSupportFunc
(
*
this
,
d
,
dIsNormalized
,
supp0
,
supp1
);
getSupportFunc
(
*
this
,
d
,
dIsNormalized
,
supp0
,
supp1
,
hint
);
}
};
...
...
@@ -123,6 +127,7 @@ struct GJK
};
typedef
unsigned
char
vertex_id_t
;
typedef
MinkowskiDiff
::
hint_t
support_hint_t
;
struct
Simplex
{
...
...
@@ -138,6 +143,7 @@ struct GJK
MinkowskiDiff
const
*
shape
;
Vec3f
ray
;
support_hint_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.
...
...
@@ -164,12 +170,14 @@ struct GJK
void
initialize
();
/// @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
());
/// @brief apply the support function along a direction, the result is return in sv
inline
void
getSupport
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
SimplexV
&
sv
)
const
inline
void
getSupport
(
const
Vec3f
&
d
,
bool
dIsNormalized
,
SimplexV
&
sv
,
support_hint_t
&
hint
)
const
{
shape
->
support
(
d
,
dIsNormalized
,
sv
.
w0
,
sv
.
w1
);
shape
->
support
(
d
,
dIsNormalized
,
sv
.
w0
,
sv
.
w1
,
hint
);
sv
.
w
.
noalias
()
=
sv
.
w0
-
sv
.
w1
;
}
...
...
@@ -229,7 +237,8 @@ private:
inline
void
removeVertex
(
Simplex
&
simplex
);
/// @brief append one vertex to the simplex
inline
void
appendVertex
(
Simplex
&
simplex
,
const
Vec3f
&
v
,
bool
isNormalized
=
false
);
inline
void
appendVertex
(
Simplex
&
simplex
,
const
Vec3f
&
v
,
bool
isNormalized
,
support_hint_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 @
35d3109b
...
...
@@ -52,6 +52,8 @@ 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
,
...
...
@@ -61,14 +63,22 @@ namespace fcl
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
Vec3f
guess
(
1
,
0
,
0
);
if
(
enable_cached_guess
)
guess
=
cached_guess
;
support_func_guess_t
support_hint
;
if
(
enable_cached_guess
)
{
guess
=
cached_guess
;
support_hint
=
support_func_cached_guess
;
}
else
support_hint
.
setZero
();
details
::
MinkowskiDiff
shape
;
shape
.
set
(
&
s1
,
&
s2
,
tf1
,
tf2
);
details
::
GJK
gjk
((
unsigned
int
)
gjk_max_iterations
,
gjk_tolerance
);
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
);
if
(
enable_cached_guess
)
cached_guess
=
gjk
.
getGuessFromSimplex
();
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
,
support_hint
);
if
(
enable_cached_guess
)
{
cached_guess
=
gjk
.
getGuessFromSimplex
();
support_func_cached_guess
=
gjk
.
support_hint
;
}
Vec3f
w0
,
w1
;
switch
(
gjk_status
)
{
...
...
@@ -127,14 +137,22 @@ namespace fcl
tf_1M2
.
transform
(
P3
));
Vec3f
guess
(
1
,
0
,
0
);
if
(
enable_cached_guess
)
guess
=
cached_guess
;
support_func_guess_t
support_hint
;
if
(
enable_cached_guess
)
{
guess
=
cached_guess
;
support_hint
=
support_func_cached_guess
;
}
else
support_hint
.
setZero
();
details
::
MinkowskiDiff
shape
;
shape
.
set
(
&
s
,
&
tri
);
details
::
GJK
gjk
((
unsigned
int
)
gjk_max_iterations
,
gjk_tolerance
);
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
);
if
(
enable_cached_guess
)
cached_guess
=
gjk
.
getGuessFromSimplex
();
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
,
support_hint
);
if
(
enable_cached_guess
)
{
cached_guess
=
gjk
.
getGuessFromSimplex
();
support_func_cached_guess
=
gjk
.
support_hint
;
}
Vec3f
w0
,
w1
;
switch
(
gjk_status
)
{
...
...
@@ -197,14 +215,22 @@ namespace fcl
FCL_REAL
eps
(
sqrt
(
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()));
#endif
Vec3f
guess
(
1
,
0
,
0
);
if
(
enable_cached_guess
)
guess
=
cached_guess
;
support_func_guess_t
support_hint
;
if
(
enable_cached_guess
)
{
guess
=
cached_guess
;
support_hint
=
support_func_cached_guess
;
}
else
support_hint
.
setZero
();
details
::
MinkowskiDiff
shape
;
shape
.
set
(
&
s1
,
&
s2
,
tf1
,
tf2
);
details
::
GJK
gjk
((
unsigned
int
)
gjk_max_iterations
,
gjk_tolerance
);
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
);
if
(
enable_cached_guess
)
cached_guess
=
gjk
.
getGuessFromSimplex
();
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
,
support_hint
);
if
(
enable_cached_guess
)
{
cached_guess
=
gjk
.
getGuessFromSimplex
();
support_func_cached_guess
=
gjk
.
support_hint
;
}
if
(
gjk_status
==
details
::
GJK
::
Failed
)
{
...
...
@@ -279,6 +305,7 @@ namespace fcl
epa_tolerance
=
1e-6
;
enable_cached_guess
=
false
;
cached_guess
=
Vec3f
(
1
,
0
,
0
);
support_func_cached_guess
=
support_func_guess_t
::
Zero
();
}
void
enableCachedGuess
(
bool
if_enable
)
const
...
...
@@ -319,6 +346,9 @@ namespace fcl
/// @brief smart guess
mutable
Vec3f
cached_guess
;
/// @brief smart guess for the support function
mutable
support_func_guess_t
support_func_cached_guess
;
};
#if __cplusplus < 201103L
...
...
src/narrowphase/gjk.cpp
View file @
35d3109b
...
...
@@ -97,7 +97,7 @@ template <> struct shape_traits<ConvexBase> : shape_traits_base
};
};
void
getShapeSupport
(
const
TriangleP
*
triangle
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
void
getShapeSupport
(
const
TriangleP
*
triangle
,
const
Vec3f
&
dir
,
Vec3f
&
support
,
int
&
)
{
FCL_REAL
dota
=
dir
.
dot
(
triangle
->
a
);
FCL_REAL
dotb
=
dir
.
dot
(
triangle
->
b
);
...
...
@@ -118,25 +118,25 @@ void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support
}
}
inline
void
getShapeSupport
(
const
Box
*
box
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
inline
void
getShapeSupport
(
const
Box
*
box
,
const
Vec3f
&
dir
,
Vec3f
&
support
,
int
&
)
{
const
FCL_REAL
inflate
=
(
dir
.
array
()
==
0
).
any
()
?
1.00000001
:
1.
;
support
.
noalias
()
=
(
dir
.
array
()
>
0
).
select
(
inflate
*
box
->
halfSide
,
-
inflate
*
box
->
halfSide
);
}
inline
void
getShapeSupport
(
const
Sphere
*
,
const
Vec3f
&
/*dir*/
,
Vec3f
&
support
)
inline
void
getShapeSupport
(
const
Sphere
*
,
const
Vec3f
&
/*dir*/
,
Vec3f
&
support
,
int
&
)
{
support
.
setZero
();
}
inline
void
getShapeSupport
(
const
Capsule
*
capsule
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
inline
void
getShapeSupport
(
const
Capsule
*
capsule
,
const
Vec3f
&
dir
,
Vec3f
&
support
,
int
&
)
{
support
.
head
<
2
>
().
setZero
();
if
(
dir
[
2
]
>
0
)
support
[
2
]
=
capsule
->
halfLength
;
else
support
[
2
]
=
-
capsule
->
halfLength
;
}
void
getShapeSupport
(
const
Cone
*
cone
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
void
getShapeSupport
(
const
Cone
*
cone
,
const
Vec3f
&
dir
,
Vec3f
&
support
,
int
&
)
{
// The cone radius is, for -h < z < h, (h - z) * r / (2*h)
static
const
FCL_REAL
inflate
=
1.00001
;
...
...
@@ -174,7 +174,7 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
}
}
void
getShapeSupport
(
const
Cylinder
*
cylinder
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
void
getShapeSupport
(
const
Cylinder
*
cylinder
,
const
Vec3f
&
dir
,
Vec3f
&
support
,
int
&
)
{
// The inflation makes the object look strictly convex to GJK and EPA. This
// helps solving particular cases (e.g. a cylinder with itself at the same
...
...
@@ -196,39 +196,40 @@ void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
<
sqrt
(
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()));
}
void
getShapeSupport
(
const
ConvexBase
*
convex
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
void
getShapeSupport
(
const
ConvexBase
*
convex
,
const
Vec3f
&
dir
,
Vec3f
&
support
,
int
&
hint
)
{
const
Vec3f
*
pts
=
convex
->
points
;
const
ConvexBase
::
Neighbors
*
nn
=
convex
->
neighbors
;
int
i
=
0
;
FCL_REAL
maxdot
=
pts
[
i
].
dot
(
dir
);
if
(
hint
<
0
||
hint
>=
convex
->
num_points
)
hint
=
0
;
FCL_REAL
maxdot
=
pts
[
hint
].
dot
(
dir
);
FCL_REAL
dot
;
bool
found
=
true
;
while
(
found
)
{
const
ConvexBase
::
Neighbors
&
n
=
nn
[
i
];
const
ConvexBase
::
Neighbors
&
n
=
nn
[
hint
];
found
=
false
;
for
(
int
in
=
0
;
in
<
n
.
count
();
++
in
)
{
dot
=
pts
[
n
[
in
]].
dot
(
dir
);
if
(
dot
>
maxdot
)
{
maxdot
=
dot
;
i
=
n
[
in
];
hint
=
n
[
in
];
found
=
true
;
}
}
}
support
=
pts
[
i
];
support
=
pts
[
hint
];
}
#define CALL_GET_SHAPE_SUPPORT(ShapeType) \
getShapeSupport (static_cast<const ShapeType*>(shape), \
(shape_traits<ShapeType>::NeedNormalizedDir && !dirIsNormalized) \
? dir.normalized() : dir, \
support)
support
, hint
)
Vec3f
getSupport
(
const
ShapeBase
*
shape
,
const
Vec3f
&
dir
,
bool
dirIsNormalized
)
Vec3f
getSupport
(
const
ShapeBase
*
shape
,
const
Vec3f
&
dir
,
bool
dirIsNormalized
,
int
&
hint
)
{
Vec3f
support
;
switch
(
shape
->
getNodeType
())
...
...
@@ -269,20 +270,22 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
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
)
const
Vec3f
&
dir
,
Vec3f
&
support0
,
Vec3f
&
support1
,
MinkowskiDiff
::
hint_t
&
hint
)
{
getShapeSupport
(
s0
,
dir
,
support0
);
getShapeSupport
(
s0
,
dir
,
support0
,
hint
[
0
]
);
if
(
TransformIsIdentity
)
getShapeSupport
(
s1
,
-
dir
,
support1
);
getShapeSupport
(
s1
,
-
dir
,
support1
,
hint
[
1
]
);
else
{
getShapeSupport
(
s1
,
-
oR1
.
transpose
()
*
dir
,
support1
);
getShapeSupport
(
s1
,
-
oR1
.
transpose
()
*
dir
,
support1
,
hint
[
1
]
);
support1
=
oR1
*
support1
+
ot1
;
}
}
template
<
typename
Shape0
,
typename
Shape1
,
bool
TransformIsIdentity
>
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
)
{
enum
{
NeedNormalizedDir
=
bool
(
(
bool
)
shape_traits
<
Shape0
>::
NeedNormalizedDir
...
...
@@ -301,7 +304,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
static_cast
<
const
Shape1
*>
(
md
.
shapes
[
1
]),
md
.
oR1
,
md
.
ot1
,
(
NeedNormalizedDir
&&
!
dirIsNormalized
)
?
dir
.
normalized
()
:
dir
,
support0
,
support1
);
support0
,
support1
,
hint
);
}
template
<
typename
Shape0
>
...
...
@@ -508,7 +511,8 @@ bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1)
return
true
;
}
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
)
{
size_t
iterations
=
0
;
FCL_REAL
alpha
=
0
;
...
...
@@ -527,9 +531,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
distance
=
0.0
;
simplices
[
0
].
rank
=
0
;
ray
=
guess
;
support_hint
=
supportHint
;
if
(
ray
.
squaredNorm
()
>
0
)
appendVertex
(
simplices
[
0
],
-
ray
);
else
appendVertex
(
simplices
[
0
],
Vec3f
(
1
,
0
,
0
),
true
);
if
(
ray
.
squaredNorm
()
>
0
)
appendVertex
(
simplices
[
0
],
-
ray
,
false
,
support_hint
);
else
appendVertex
(
simplices
[
0
],
Vec3f
(
1
,
0
,
0
),
true
,
support_hint
);
ray
=
simplices
[
0
].
vertex
[
0
]
->
w
;
FCL_REAL
rl
=
ray
.
norm
();
...
...
@@ -558,7 +563,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
break
;
}
appendVertex
(
curr_simplex
,
-
ray
);
// see below, ray points away from origin
appendVertex
(
curr_simplex
,
-
ray
,
false
,
support_hint
);
// see below, ray points away from origin
// check removed (by ?): when the new support point is close to previous support points, stop (as the new simplex is degenerated)
const
Vec3f
&
w
=
curr_simplex
.
vertex
[
curr_simplex
.
rank
-
1
]
->
w
;
...
...
@@ -622,26 +627,27 @@ inline void GJK::removeVertex(Simplex& simplex)
free_v
[
nfree
++
]
=
simplex
.
vertex
[
--
simplex
.
rank
];
}
inline
void
GJK
::
appendVertex
(
Simplex
&
simplex
,
const
Vec3f
&
v
,
bool
isNormalized
)
inline
void
GJK
::
appendVertex
(
Simplex
&
simplex
,
const
Vec3f
&
v
,
bool
isNormalized
,
support_hint_t
&
hint
)
{
simplex
.
vertex
[
simplex
.
rank
]
=
free_v
[
--
nfree
];
// set the memory
getSupport
(
v
,
isNormalized
,
*
simplex
.
vertex
[
simplex
.
rank
++
]);
getSupport
(
v
,
isNormalized
,
*
simplex
.
vertex
[
simplex
.
rank
++
]
,
hint
);
}
bool
GJK
::
encloseOrigin
()
{
Vec3f
axis
(
Vec3f
::
Zero
());
support_hint_t
hint
=
support_hint_t
::
Zero
();
switch
(
simplex
->
rank
)
{
case
1
:
for
(
size_t
i
=
0
;
i
<
3
;
++
i
)
{
axis
[
i
]
=
1
;
appendVertex
(
*
simplex
,
axis
,
true
);
appendVertex
(
*
simplex
,
axis
,
true
,
hint
);
if
(
encloseOrigin
())
return
true
;
removeVertex
(
*
simplex
);
axis
[
i
]
=
-
1
;
appendVertex
(
*
simplex
,
-
axis
,
true
);
appendVertex
(
*
simplex
,
-
axis
,
true
,
hint
);
if
(
encloseOrigin
())
return
true
;
removeVertex
(
*
simplex
);
axis
[
i
]
=
0
;
...
...
@@ -656,10 +662,10 @@ bool GJK::encloseOrigin()
Vec3f
p
=
d
.
cross
(
axis
);
if
(
!
p
.
isZero
())
{
appendVertex
(
*
simplex
,
p
);
appendVertex
(
*
simplex
,
p
,
false
,
hint
);
if
(
encloseOrigin
())
return
true
;
removeVertex
(
*
simplex
);
appendVertex
(
*
simplex
,
-
p
);
appendVertex
(
*
simplex
,
-
p
,
false
,
hint
);
if
(
encloseOrigin
())
return
true
;
removeVertex
(
*
simplex
);
}
...
...
@@ -673,10 +679,10 @@ bool GJK::encloseOrigin()
(
simplex
->
vertex
[
2
]
->
w
-
simplex
->
vertex
[
0
]
->
w
);
if
(
!
axis
.
isZero
())
{
appendVertex
(
*
simplex
,
axis
);
appendVertex
(
*
simplex
,
axis
,
false
,
hint
);
if
(
encloseOrigin
())
return
true
;
removeVertex
(
*
simplex
);
appendVertex
(
*
simplex
,
-
axis
);
appendVertex
(
*
simplex
,
-
axis
,
false
,
hint
);
if
(
encloseOrigin
())
return
true
;
removeVertex
(
*
simplex
);
}
...
...
@@ -1258,6 +1264,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
);
if
((
simplex
.
rank
>
1
)
&&
gjk
.
encloseOrigin
())
{
while
(
hull
.
root
)
...
...
@@ -1316,7 +1323,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
bool
valid
=
true
;
best
->
pass
=
++
pass
;
// At the moment, SimplexF.n is always normalized. This could be revised in the future...
gjk
.
getSupport
(
best
->
n
,
true
,
*
w
);
gjk
.
getSupport
(
best
->
n
,
true
,
*
w
,
hint
);
FCL_REAL
wdist
=
best
->
n
.
dot
(
w
->
w
)
-
best
->
d
;
if
(
wdist
<=
tolerance
)
{
status
=
AccuracyReached
;
...
...
src/narrowphase/narrowphase.cpp
View file @
35d3109b
...
...
@@ -496,16 +496,25 @@ bool GJKSolver::shapeDistance<Capsule, Capsule>
t2
(
tf2
.
transform
(
s2
.
a
),
tf2
.
transform
(
s2
.
b
),
tf2
.
transform
(
s2
.
c
));
Vec3f
guess
;
if
(
enable_cached_guess
)
guess
=
cached_guess
;
else
guess
=
(
t1
.
a
+
t1
.
b
+
t1
.
c
-
t2
.
a
-
t2
.
b
-
t2
.
c
)
/
3
;
support_func_guess_t
support_hint
;
if
(
enable_cached_guess
)
{
guess
=
cached_guess
;
support_hint
=
support_func_cached_guess
;
}
else
{
support_hint
.
setZero
();
guess
=
(
t1
.
a
+
t1
.
b
+
t1
.
c
-
t2
.
a
-
t2
.
b
-
t2
.
c
)
/
3
;
}
bool
enable_penetration
=
true
;
details
::
MinkowskiDiff
shape
;
shape
.
set
(
&
t1
,
&
t2
);
details
::
GJK
gjk
((
unsigned
int
)
gjk_max_iterations
,
gjk_tolerance
);
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
);
if
(
enable_cached_guess
)
cached_guess
=
gjk
.
getGuessFromSimplex
();
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
,
support_hint
);
if
(
enable_cached_guess
)
{
cached_guess
=
gjk
.
getGuessFromSimplex
();
support_func_cached_guess
=
gjk
.
support_hint
;
}
gjk
.
getClosestPoints
(
shape
,
p1
,
p2
);
...
...
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