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
829b33c6
Commit
829b33c6
authored
Apr 03, 2020
by
Joseph Mirabel
Browse files
shapeIntersect handle EPA failure.
parent
26f7e0be
Changes
1
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/narrowphase.h
View file @
829b33c6
...
...
@@ -148,13 +148,21 @@ namespace fcl
}
else
{
details
::
EPA
epa
(
epa_max_face_num
,
epa_max_vertex_num
,
epa_max_iterations
,
epa_tolerance
);
details
::
EPA
::
Status
epa_status
=
epa
.
evaluate
(
gjk
,
-
guess
);
assert
(
epa_status
&
details
::
EPA
::
Valid
);
(
void
)
epa_status
;
epa
.
getClosestPoints
(
shape
,
w0
,
w1
);
distance
=
-
epa
.
depth
;
normal
=
-
epa
.
normal
;
p1
=
p2
=
tf1
.
transform
(
w0
-
epa
.
normal
*
(
epa
.
depth
*
0.5
));
assert
(
distance
<=
1e-6
);
if
(
epa_status
&
details
::
EPA
::
Valid
||
epa_status
==
details
::
EPA
::
OutOfFaces
// Warnings
||
epa_status
==
details
::
EPA
::
OutOfVertices
// Warnings
)
{
epa
.
getClosestPoints
(
shape
,
w0
,
w1
);
distance
=
-
epa
.
depth
;
normal
=
-
epa
.
normal
;
p1
=
p2
=
tf1
.
transform
(
w0
-
epa
.
normal
*
(
epa
.
depth
*
0.5
));
assert
(
distance
<=
1e-6
);
}
else
{
distance
=
-
std
::
numeric_limits
<
FCL_REAL
>::
max
();
gjk
.
getClosestPoints
(
shape
,
w0
,
w1
);
p1
=
p2
=
tf1
.
transform
(
w0
);
}
}
break
;
case
details
::
GJK
::
Valid
:
...
...
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