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
6ce7e41a
Unverified
Commit
6ce7e41a
authored
Jun 10, 2021
by
Justin Carpentier
Committed by
GitHub
Jun 10, 2021
Browse files
Merge pull request #231 from jcarpent/devel
Additional fix for collision checking
parents
c21cebf4
3a574ad3
Pipeline
#14900
passed with stage
in 40 minutes and 4 seconds
Changes
2
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
src/collision_func_matrix.cpp
View file @
6ce7e41a
...
...
@@ -97,7 +97,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
}
num_contacts
=
result
.
numContacts
();
}
if
(
distance
<=
request
.
security_margin
)
{
else
if
(
distance
<=
request
.
security_margin
)
{
if
(
result
.
numContacts
()
<
request
.
num_max_contacts
)
{
const
Vec3f
&
p1
=
distanceResult
.
nearest_points
[
0
];
const
Vec3f
&
p2
=
distanceResult
.
nearest_points
[
1
];
...
...
src/distance/sphere_sphere.cpp
View file @
6ce7e41a
...
...
@@ -121,7 +121,7 @@ namespace fcl {
unit
=
c1c2
/
dist
;
// Unlike in distance computation, we consider the security margin.
FCL_REAL
penetrationDepth
=
r1
+
r2
+
margin
-
dist
;
result
.
updateDistanceLowerBound
(
-
penetrationDepth
);
result
.
updateDistanceLowerBound
(
-
penetrationDepth
+
margin
);
bool
collision
=
(
penetrationDepth
>=
0
);
if
(
collision
)
{
// Take contact point at the middle of intersection between each sphere
...
...
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