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
37f46961
Verified
Commit
37f46961
authored
Jun 09, 2021
by
Justin Carpentier
Browse files
collision: update distance lower bound
parent
b0d0046a
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/collision_func_matrix.cpp
View file @
37f46961
...
...
@@ -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
...
...
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