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
bdfa884d
Unverified
Commit
bdfa884d
authored
Jun 09, 2021
by
Justin Carpentier
Committed by
GitHub
Jun 09, 2021
Browse files
Merge pull request #228 from jcarpent/devel
Fix update of distanceLowerBound
parents
19d44ba2
19918b9e
Pipeline
#14888
passed with stage
in 90 minutes and 33 seconds
Changes
4
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
cmake
@
09483408
Compare
3a52692a
...
09483408
Subproject commit
3a52692a40839b10f38352c1b06ccfebc0b53f36
Subproject commit
094834088ad9de32f1abdc2d56d29bca2190772c
include/hpp/fcl/collision_data.h
View file @
bdfa884d
...
...
@@ -285,7 +285,7 @@ public:
{
}
/// @brief Update the lower bound only if the distance i
n
inferior.
/// @brief Update the lower bound only if the distance i
s
inferior.
inline
void
updateDistanceLowerBound
(
const
FCL_REAL
&
distance_lower_bound_
)
{
if
(
distance_lower_bound_
<
distance_lower_bound
)
...
...
src/collision_func_matrix.cpp
View file @
bdfa884d
...
...
@@ -82,6 +82,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
FCL_REAL
distance
=
ShapeShapeDistance
<
T_SH1
,
T_SH2
>
(
o1
,
tf1
,
o2
,
tf2
,
nsolver
,
distanceRequest
,
distanceResult
);
size_t
num_contacts
=
0
;
if
(
distance
<=
0
)
{
if
(
result
.
numContacts
()
<
request
.
num_max_contacts
)
{
const
Vec3f
&
p1
=
distanceResult
.
nearest_points
[
0
];
...
...
@@ -94,7 +95,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
result
.
addContact
(
contact
);
}
return
1
;
num_contacts
=
result
.
numContacts
()
;
}
if
(
distance
<=
request
.
security_margin
)
{
if
(
result
.
numContacts
()
<
request
.
num_max_contacts
)
{
...
...
@@ -107,10 +108,10 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
-
distance
+
request
.
security_margin
);
result
.
addContact
(
contact
);
}
return
1
;
num_contacts
=
result
.
numContacts
()
;
}
result
.
updateDistanceLowerBound
(
distance
);
return
0
;
return
num_contacts
;
}
namespace
details
...
...
test/distance_lower_bound.cpp
View file @
bdfa884d
...
...
@@ -151,7 +151,7 @@ BOOST_AUTO_TEST_CASE(mesh_mesh)
col1
=
testDistanceLowerBound
(
transforms
[
i
],
m1
,
m2
,
distanceLowerBound
);
col2
=
testDistance
(
transforms
[
i
],
m1
,
m2
,
distance
);
col3
=
testCollide
(
transforms
[
i
],
m1
,
m2
);
std
::
cout
<<
"col1 = "
<<
col1
<<
", col2 = "
<<
col2
std
::
cout
<<
"col1 = "
<<
col1
<<
", col2 = "
<<
col2
<<
", col3 = "
<<
col3
<<
std
::
endl
;
std
::
cout
<<
"distance lower bound = "
<<
distanceLowerBound
<<
std
::
endl
;
std
::
cout
<<
"distance = "
<<
distance
<<
std
::
endl
;
...
...
@@ -164,6 +164,42 @@ BOOST_AUTO_TEST_CASE(mesh_mesh)
}
}
BOOST_AUTO_TEST_CASE
(
box_sphere
)
{
shared_ptr
<
hpp
::
fcl
::
Sphere
>
sphere
(
new
hpp
::
fcl
::
Sphere
(
0.5
));
shared_ptr
<
hpp
::
fcl
::
Box
>
box
(
new
hpp
::
fcl
::
Box
(
1.
,
1.
,
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
,
sphere
,
box
,
distanceLowerBound
);
col2
=
testDistance
(
M1
,
sphere
,
box
,
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
],
sphere
,
box
,
distanceLowerBound
);
col2
=
testDistance
(
transforms
[
i
],
sphere
,
box
,
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
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