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
f5e1a892
Commit
f5e1a892
authored
Jun 24, 2020
by
Joseph Mirabel
Browse files
Fix nearest point calculation in shapeDistance.
parent
c9292413
Changes
1
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/narrowphase.h
View file @
f5e1a892
...
...
@@ -234,7 +234,8 @@ namespace fcl
Vec3f
w0
,
w1
;
gjk
.
getClosestPoints
(
shape
,
w0
,
w1
);
distance
=
0
;
p1
=
p2
=
tf1
.
transform
(
.5
*
(
w0
+
w1
));
p1
=
tf1
.
transform
(
w0
);
p2
=
tf1
.
transform
(
w1
);
normal
=
Vec3f
(
0
,
0
,
0
);
return
false
;
}
...
...
@@ -261,7 +262,8 @@ namespace fcl
//p1 = tf1.transform (p1);
//p2 = tf1.transform (p2);
normal
=
(
tf1
.
getRotation
()
*
(
p2
-
p1
)).
normalized
();
p1
=
p2
=
tf1
.
transform
(
p1
);
p1
=
tf1
.
transform
(
p1
);
p2
=
tf1
.
transform
(
p2
);
}
else
{
details
::
EPA
epa
(
epa_max_face_num
,
epa_max_vertex_num
,
epa_max_iterations
,
epa_tolerance
);
...
...
@@ -276,12 +278,14 @@ namespace fcl
assert
(
epa
.
depth
>=
-
eps
);
distance
=
(
std
::
min
)
(
0.
,
-
epa
.
depth
);
normal
=
tf1
.
getRotation
()
*
epa
.
normal
;
p1
=
p2
=
tf1
.
transform
(
w0
-
epa
.
normal
*
(
epa
.
depth
*
0.5
));
p1
=
tf1
.
transform
(
w0
);
p2
=
tf1
.
transform
(
w1
);
return
false
;
}
distance
=
-
(
std
::
numeric_limits
<
FCL_REAL
>::
max
)();
gjk
.
getClosestPoints
(
shape
,
p1
,
p2
);
p1
=
p2
=
tf1
.
transform
(
p1
);
p1
=
tf1
.
transform
(
p1
);
p2
=
tf1
.
transform
(
p2
);
}
return
false
;
}
...
...
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