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
88cc70a3
Commit
88cc70a3
authored
Jul 16, 2020
by
Joseph Mirabel
Browse files
Fix caculation of normals when using GJK / EPA.
parent
ae22d16a
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/narrowphase.h
View file @
88cc70a3
...
...
@@ -156,7 +156,7 @@ namespace fcl
if
(
gjk
.
hasPenetrationInformation
(
shape
))
{
gjk
.
getClosestPoints
(
shape
,
w0
,
w1
);
distance
=
gjk
.
distance
;
normal
=
tf1
.
getRotation
()
*
(
w
1
-
w
0
).
normalized
();
normal
=
tf1
.
getRotation
()
*
(
w
0
-
w
1
).
normalized
();
p1
=
p2
=
tf1
.
transform
((
w0
+
w1
)
/
2
);
}
else
{
details
::
EPA
epa
(
epa_max_face_num
,
epa_max_vertex_num
,
epa_max_iterations
,
epa_tolerance
);
...
...
@@ -168,7 +168,7 @@ namespace fcl
{
epa
.
getClosestPoints
(
shape
,
w0
,
w1
);
distance
=
-
epa
.
depth
;
normal
=
-
epa
.
normal
;
normal
=
tf1
.
getRotation
()
*
epa
.
normal
;
p1
=
p2
=
tf1
.
transform
(
w0
-
epa
.
normal
*
(
epa
.
depth
*
0.5
));
assert
(
distance
<=
1e-6
);
}
else
{
...
...
@@ -261,7 +261,7 @@ namespace fcl
// Return contact points in case of collision
//p1 = tf1.transform (p1);
//p2 = tf1.transform (p2);
normal
=
(
tf1
.
getRotation
()
*
(
p
2
-
p
1
)).
normalized
();
normal
=
(
tf1
.
getRotation
()
*
(
p
1
-
p
2
)).
normalized
();
p1
=
tf1
.
transform
(
p1
);
p2
=
tf1
.
transform
(
p2
);
}
else
{
...
...
src/narrowphase/gjk.cpp
View file @
88cc70a3
...
...
@@ -575,7 +575,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
alpha
=
std
::
max
(
alpha
,
omega
);
FCL_REAL
diff
(
rl
-
alpha
);
if
(
iterations
==
0
)
diff
=
std
::
abs
(
diff
);
if
(
diff
-
tolerance
*
rl
<=
0
)
// TODO here, we can stop at iteration 0 if this condition is met.
// We stopping at iteration 0, the closest point will not be valid.
// if(diff - tolerance * rl <= 0)
if
(
iterations
>
0
&&
diff
-
tolerance
*
rl
<=
0
)
{
if
(
iterations
>
0
)
removeVertex
(
simplices
[
current
]);
...
...
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