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
90bd15ff
Commit
90bd15ff
authored
Aug 30, 2019
by
Joseph Mirabel
Browse files
[GJK] Make closest points more robust.
parent
11c6b0eb
Changes
4
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/gjk.h
View file @
90bd15ff
...
...
@@ -169,7 +169,7 @@ struct GJK
/// Get the closest points on each object.
/// \return true on success
bool
getClosestPoints
(
const
MinkowskiDiff
&
shape
,
Vec3f
&
w0
,
Vec3f
&
w1
)
const
;
static
bool
getClosestPoints
(
const
Simplex
&
simplex
,
Vec3f
&
w0
,
Vec3f
&
w1
);
/// @brief get the guess from current simplex
Vec3f
getGuessFromSimplex
()
const
;
...
...
include/hpp/fcl/narrowphase/narrowphase.h
View file @
90bd15ff
...
...
@@ -77,11 +77,8 @@ namespace fcl
details
::
EPA
::
Status
epa_status
=
epa
.
evaluate
(
gjk
,
-
guess
);
if
(
epa_status
!=
details
::
EPA
::
Failed
)
{
Vec3f
w0
(
Vec3f
::
Zero
());
for
(
size_t
i
=
0
;
i
<
epa
.
result
.
rank
;
++
i
)
{
w0
+=
epa
.
result
.
vertex
[
i
]
->
w0
*
epa
.
result
.
coefficient
[
i
];
}
Vec3f
w0
,
w1
;
details
::
GJK
::
getClosestPoints
(
epa
.
result
,
w0
,
w1
);
if
(
penetration_depth
)
*
penetration_depth
=
-
epa
.
depth
;
if
(
normal
)
*
normal
=
tf2
.
getRotation
()
*
epa
.
normal
;
if
(
contact_points
)
*
contact_points
=
tf1
.
transform
(
w0
-
epa
.
normal
*
(
epa
.
depth
*
0.5
));
...
...
@@ -127,11 +124,8 @@ namespace fcl
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
::
Failed
);
(
void
)
epa_status
;
Vec3f
w0
(
Vec3f
::
Zero
());
for
(
short
i
=
0
;
i
<
epa
.
result
.
rank
;
++
i
)
{
w0
+=
epa
.
result
.
vertex
[
i
]
->
w0
*
epa
.
result
.
coefficient
[
i
];
}
Vec3f
w0
,
w1
;
details
::
GJK
::
getClosestPoints
(
epa
.
result
,
w0
,
w1
);
distance
=
-
epa
.
depth
;
normal
=
-
epa
.
normal
;
p1
=
p2
=
tf1
.
transform
(
w0
-
epa
.
normal
*
(
epa
.
depth
*
0.5
));
...
...
@@ -143,7 +137,7 @@ namespace fcl
{
col
=
false
;
gjk
.
getClosestPoints
(
shape
,
p1
,
p2
);
details
::
GJK
::
getClosestPoints
(
*
gjk
.
getSimplex
()
,
p1
,
p2
);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
...
...
@@ -188,7 +182,7 @@ namespace fcl
// TODO: understand why GJK fails between cylinder and box
assert
(
distance
*
distance
<
sqrt
(
eps
));
Vec3f
w0
,
w1
;
gjk
.
getClosestPoints
(
shape
,
w0
,
w1
);
details
::
GJK
::
getClosestPoints
(
*
gjk
.
getSimplex
()
,
w0
,
w1
);
distance
=
0
;
p1
=
p2
=
tf1
.
transform
(
.5
*
(
w0
+
w1
));
normal
=
Vec3f
(
0
,
0
,
0
);
...
...
@@ -196,7 +190,7 @@ namespace fcl
}
else
if
(
gjk_status
==
details
::
GJK
::
Valid
)
{
gjk
.
getClosestPoints
(
shape
,
p1
,
p2
);
details
::
GJK
::
getClosestPoints
(
*
gjk
.
getSimplex
()
,
p1
,
p2
);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
...
...
@@ -216,11 +210,8 @@ namespace fcl
details
::
EPA
::
Status
epa_status
=
epa
.
evaluate
(
gjk
,
-
guess
);
if
(
epa_status
!=
details
::
EPA
::
Failed
)
{
Vec3f
w0
(
Vec3f
::
Zero
());
for
(
short
i
=
0
;
i
<
epa
.
result
.
rank
;
++
i
)
{
w0
+=
epa
.
result
.
vertex
[
i
]
->
w0
*
epa
.
result
.
coefficient
[
i
];
}
Vec3f
w0
,
w1
;
details
::
GJK
::
getClosestPoints
(
epa
.
result
,
w0
,
w1
);
assert
(
epa
.
depth
>=
-
eps
);
distance
=
std
::
min
(
0.
,
-
epa
.
depth
);
normal
=
tf2
.
getRotation
()
*
epa
.
normal
;
...
...
@@ -229,7 +220,7 @@ namespace fcl
}
else
{
gjk
.
getClosestPoints
(
shape
,
p1
,
p2
);
details
::
GJK
::
getClosestPoints
(
*
gjk
.
getSimplex
()
,
p1
,
p2
);
distance
=
0
;
p1
=
tf1
.
transform
(
p1
);
...
...
src/narrowphase/gjk.cpp
View file @
90bd15ff
...
...
@@ -308,15 +308,59 @@ Vec3f GJK::getGuessFromSimplex() const
return
ray
;
}
bool
GJK
::
getClosestPoints
(
const
MinkowskiDiff
&
shape
,
Vec3f
&
w0
,
Vec3f
&
w1
)
const
bool
GJK
::
getClosestPoints
(
const
Simplex
&
simplex
,
Vec3f
&
w0
,
Vec3f
&
w1
)
{
SimplexV
*
const
*
vs
=
simplex
.
vertex
;
for
(
short
i
=
0
;
i
<
simplex
.
rank
;
++
i
)
{
assert
(
vs
[
i
]
->
w
.
isApprox
(
vs
[
i
]
->
w0
-
vs
[
i
]
->
w1
));
}
Project
::
ProjectResult
projection
;
switch
(
simplex
.
rank
)
{
case
1
:
w0
=
vs
[
0
]
->
w0
;
w1
=
vs
[
0
]
->
w1
;
return
true
;
case
2
:
{
const
Vec3f
&
a
=
vs
[
0
]
->
w
,
a0
=
vs
[
0
]
->
w0
,
a1
=
vs
[
0
]
->
w1
,
b
=
vs
[
1
]
->
w
,
b0
=
vs
[
1
]
->
w0
,
b1
=
vs
[
1
]
->
w1
;
FCL_REAL
la
,
lb
;
Vec3f
N
(
b
-
a
);
la
=
N
.
dot
(
-
a
);
if
(
la
<=
0
)
{
w0
=
a0
;
w1
=
a1
;
}
else
{
lb
=
N
.
squaredNorm
();
if
(
la
>
lb
)
{
w0
=
b0
;
w1
=
b1
;
}
else
{
lb
=
la
/
lb
;
la
=
1
-
lb
;
w0
=
la
*
a0
+
lb
*
b0
;
w1
=
la
*
a1
+
lb
*
b1
;
}
}
}
return
true
;
case
3
:
// TODO avoid the reprojection
projection
=
Project
::
projectTriangleOrigin
(
vs
[
0
]
->
w
,
vs
[
1
]
->
w
,
vs
[
2
]
->
w
);
break
;
case
4
:
// We are in collision.
projection
=
Project
::
projectTetrahedraOrigin
(
vs
[
0
]
->
w
,
vs
[
1
]
->
w
,
vs
[
2
]
->
w
,
vs
[
3
]
->
w
);
break
;
default:
throw
std
::
logic_error
(
"The simplex rank must be in [ 1, 4 ]"
);
}
w0
.
setZero
();
w1
.
setZero
();
for
(
short
i
=
0
;
i
<
getSimplex
()
->
rank
;
++
i
)
{
FCL_REAL
p
=
getSimplex
()
->
coefficient
[
i
];
w0
+=
getSimplex
()
->
vertex
[
i
]
->
w0
*
p
;
w1
+=
getSimplex
()
->
vertex
[
i
]
->
w1
*
p
;
for
(
short
i
=
0
;
i
<
simplex
.
rank
;
++
i
)
{
w0
+=
projection
.
parameterization
[
i
]
*
vs
[
i
]
->
w0
;
w1
+=
projection
.
parameterization
[
i
]
*
vs
[
i
]
->
w1
;
}
return
true
;
}
...
...
src/narrowphase/narrowphase.cpp
View file @
90bd15ff
...
...
@@ -644,7 +644,7 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
);
if
(
enable_cached_guess
)
cached_guess
=
gjk
.
getGuessFromSimplex
();
gjk
.
getClosestPoints
(
shape
,
p1
,
p2
);
details
::
GJK
::
getClosestPoints
(
*
gjk
.
getSimplex
()
,
p1
,
p2
);
if
((
gjk_status
==
details
::
GJK
::
Valid
)
||
(
gjk_status
==
details
::
GJK
::
Failed
))
...
...
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