Skip to content
GitLab
Menu
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
d1c7b905
Verified
Commit
d1c7b905
authored
Jun 09, 2021
by
Justin Carpentier
Browse files
collision: update distance lower bound for sphere-sphere case
parent
85182356
Pipeline
#14894
passed with stage
in 35 minutes and 40 seconds
Changes
2
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
src/distance/sphere_sphere.cpp
View file @
d1c7b905
...
...
@@ -119,9 +119,9 @@ namespace fcl {
Vec3f
unit
(
0
,
0
,
0
);
if
(
dist
>
epsilon
)
unit
=
c1c2
/
dist
;
FCL_REAL
penetrationDepth
;
// Unlike in distance computation, we consider the security margin.
penetrationDepth
=
r1
+
r2
+
margin
-
dist
;
FCL_REAL
penetrationDepth
=
r1
+
r2
+
margin
-
dist
;
result
.
updateDistanceLowerBound
(
-
penetrationDepth
);
bool
collision
=
(
penetrationDepth
>=
0
);
if
(
collision
)
{
// Take contact point at the middle of intersection between each sphere
...
...
@@ -132,7 +132,6 @@ namespace fcl {
result
.
addContact
(
contact
);
return
1
;
}
result
.
updateDistanceLowerBound
(
-
penetrationDepth
);
return
0
;
}
}
// namespace fcl
...
...
test/distance_lower_bound.cpp
View file @
d1c7b905
...
...
@@ -200,6 +200,42 @@ BOOST_AUTO_TEST_CASE(box_sphere)
}
}
BOOST_AUTO_TEST_CASE
(
sphere_sphere
)
{
shared_ptr
<
hpp
::
fcl
::
Sphere
>
sphere1
(
new
hpp
::
fcl
::
Sphere
(
0.5
));
shared_ptr
<
hpp
::
fcl
::
Sphere
>
sphere2
(
new
hpp
::
fcl
::
Sphere
(
1.
));
Transform3f
M1
;
M1
.
setIdentity
();
Transform3f
M2
;
M2
.
setIdentity
();
std
::
vector
<
Transform3f
>
transforms
;
FCL_REAL
extents
[]
=
{
-
2.
,
-
2.
,
-
2.
,
2.
,
2.
,
2.
};
const
std
::
size_t
n
=
1000
;
generateRandomTransforms
(
extents
,
transforms
,
n
);
FCL_REAL
distanceLowerBound
,
distance
;
bool
col1
,
col2
;
col1
=
testDistanceLowerBound
(
M1
,
sphere1
,
sphere2
,
distanceLowerBound
);
col2
=
testDistance
(
M1
,
sphere1
,
sphere2
,
distance
);
BOOST_CHECK
(
col1
==
col2
);
BOOST_CHECK
(
distanceLowerBound
<=
distance
);
for
(
std
::
size_t
i
=
0
;
i
<
transforms
.
size
();
++
i
)
{
FCL_REAL
distanceLowerBound
,
distance
;
bool
col1
,
col2
;
col1
=
testDistanceLowerBound
(
transforms
[
i
],
sphere1
,
sphere2
,
distanceLowerBound
);
col2
=
testDistance
(
transforms
[
i
],
sphere1
,
sphere2
,
distance
);
BOOST_CHECK
(
col1
==
col2
);
if
(
!
col1
)
{
BOOST_CHECK_MESSAGE
(
distanceLowerBound
<=
distance
,
"distance = "
<<
distance
<<
", lower bound = "
<<
distanceLowerBound
);
}
}
}
BOOST_AUTO_TEST_CASE
(
box_mesh
)
{
std
::
vector
<
Vec3f
>
p1
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a 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