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
Guilhem Saurel
hpp-fcl
Commits
e2bdabcb
Commit
e2bdabcb
authored
Mar 31, 2020
by
Joseph Mirabel
Browse files
Update API of GJKSolver::shapeIntersect
parent
c90803a0
Changes
2
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/narrowphase.h
View file @
e2bdabcb
...
...
@@ -56,7 +56,9 @@ namespace fcl
template
<
typename
S1
,
typename
S2
>
bool
shapeIntersect
(
const
S1
&
s1
,
const
Transform3f
&
tf1
,
const
S2
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance_lower_bound
,
bool
enable_penetration
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
Vec3f
guess
(
1
,
0
,
0
);
if
(
enable_cached_guess
)
guess
=
cached_guess
;
...
...
@@ -71,9 +73,11 @@ namespace fcl
Vec3f
w0
,
w1
;
switch
(
gjk_status
)
{
case
details
::
GJK
::
Inside
:
if
(
!
enable_penetration
&&
contact_points
==
NULL
&&
normal
==
NULL
)
return
true
;
if
(
gjk
.
hasPenetrationInformation
(
shape
))
{
gjk
.
getClosestPoints
(
shape
,
w0
,
w1
);
if
(
penetration_depth
)
*
penetration_depth
=
gjk
.
distance
;
distance_lower_bound
=
gjk
.
distance
;
if
(
normal
)
*
normal
=
tf1
.
getRotation
()
*
(
w0
-
w1
).
normalized
();
if
(
contact_points
)
*
contact_points
=
tf1
.
transform
((
w0
+
w1
)
/
2
);
return
true
;
...
...
@@ -86,16 +90,19 @@ namespace fcl
)
{
epa
.
getClosestPoints
(
shape
,
w0
,
w1
);
if
(
penetration_depth
)
*
penetration_depth
=
-
epa
.
depth
;
distance_lower_bound
=
-
epa
.
depth
;
if
(
normal
)
*
normal
=
tf1
.
getRotation
()
*
epa
.
normal
;
if
(
contact_points
)
*
contact_points
=
tf1
.
transform
(
w0
-
epa
.
normal
*
(
epa
.
depth
*
0.5
));
return
true
;
}
if
(
penetration_depth
)
*
penetration_depth
=
-
std
::
numeric_limits
<
FCL_REAL
>::
max
();
distance_lower_bound
=
-
std
::
numeric_limits
<
FCL_REAL
>::
max
();
// EPA failed but we know there is a collision so we should
return
true
;
}
break
;
case
details
::
GJK
::
Valid
:
distance_lower_bound
=
gjk
.
distance
;
break
;
default:
;
}
...
...
@@ -315,7 +322,8 @@ namespace fcl
bool GJKSolver::shapeIntersect<Shape1, Shape2> \
(const Shape1& s1, const Transform3f& tf1, \
const Shape2& s2, const Transform3f& tf2, \
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
FCL_REAL& distance_lower_bound, bool enable_penetration, \
Vec3f* contact_points, Vec3f* normal) const
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_SELF(Shape,doc) \
HPP_FCL_DECLARE_SHAPE_INTERSECT(Shape,Shape,doc)
#define HPP_FCL_DECLARE_SHAPE_INTERSECT_PAIR(Shape1,Shape2,doc) \
...
...
src/narrowphase/narrowphase.cpp
View file @
e2bdabcb
...
...
@@ -78,69 +78,77 @@ namespace fcl
bool GJKSolver::shapeIntersect<Shape1, Shape2> \
(const Shape1& s1, const Transform3f& tf1, \
const Shape2& s2, const Transform3f& tf2, \
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const \
FCL_REAL& distance_lower_bound, bool enable_penetration, \
Vec3f* contact_points, Vec3f* normal) const \
{ \
bool res = shapeIntersect (s2, tf2, s1, tf1,
contact_points,
\
penetration
_depth, normal);
\
bool res = shapeIntersect (s2, tf2, s1, tf1,
distance_lower_bound,
\
enable_
penetration
, contact_points, normal);
\
(*normal) *= -1.0; \
return res; \
}
template
<
>
bool
GJKSolver
::
shapeIntersect
<
Sphere
,
Capsule
>
(
const
Sphere
&
s1
,
const
Transform3f
&
tf1
,
const
Capsule
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
const
Capsule
&
s2
,
const
Transform3f
&
tf2
,
FCL_REAL
&
distance_lower_bound
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
return
details
::
sphereCapsuleIntersect
(
s1
,
tf1
,
s2
,
tf2
,
contact_points
,
penetration_depth
,
normal
);
return
details
::
sphereCapsuleIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance_lower_bound
,
contact_points
,
normal
);
}
SHAPE_INTERSECT_INVERTED
(
Capsule
,
Sphere
)
template
<
>
bool
GJKSolver
::
shapeIntersect
<
Sphere
,
Sphere
>
(
const
Sphere
&
s1
,
const
Transform3f
&
tf1
,
const
Sphere
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
const
Sphere
&
s2
,
const
Transform3f
&
tf2
,
FCL_REAL
&
distance_lower_bound
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
return
details
::
sphereSphereIntersect
(
s1
,
tf1
,
s2
,
tf2
,
contact_points
,
penetration_depth
,
normal
);
return
details
::
sphereSphereIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance_lower_bound
,
contact_points
,
normal
);
}
template
<
>
bool
GJKSolver
::
shapeIntersect
<
Box
,
Sphere
>
(
const
Box
&
s1
,
const
Transform3f
&
tf1
,
const
Sphere
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
bool
GJKSolver
::
shapeIntersect
<
Box
,
Sphere
>
(
const
Box
&
s1
,
const
Transform3f
&
tf1
,
const
Sphere
&
s2
,
const
Transform3f
&
tf2
,
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
dist
;
Vec3f
ps
,
pb
,
n
;
bool
intersect
=
details
::
boxSphereDistance
(
s1
,
tf1
,
s2
,
tf2
,
dist
,
ps
,
pb
,
n
);
if
(
!
intersect
)
return
false
;
if
(
penetration_depth
)
*
penetration_depth
=
dist
;
bool
res
=
details
::
boxSphereDistance
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
ps
,
pb
,
n
);
if
(
normal
)
*
normal
=
n
;
if
(
contact_points
)
*
contact_points
=
pb
;
return
true
;
return
res
;
}
SHAPE_INTERSECT_INVERTED
(
Sphere
,
Box
)
template
<
>
bool
GJKSolver
::
shapeIntersect
<
Box
,
Box
>
(
const
Box
&
s1
,
const
Transform3f
&
tf1
,
const
Box
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
const
Box
&
s2
,
const
Transform3f
&
tf2
,
FCL_REAL
&
distance_lower_bound
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
return
details
::
boxBoxIntersect
(
s1
,
tf1
,
s2
,
tf2
,
contact_points
,
penetration_depth
,
normal
);
return
details
::
boxBoxIntersect
(
s1
,
tf1
,
s2
,
tf2
,
contact_points
,
&
distance_lower_bound
,
normal
);
}
template
<
>
bool
GJKSolver
::
shapeIntersect
<
Sphere
,
Halfspace
>
(
const
Sphere
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
sphereHalfspaceIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
...
...
@@ -150,14 +158,14 @@ template<>
bool
GJKSolver
::
shapeIntersect
<
Box
,
Halfspace
>
(
const
Box
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
boxHalfspaceIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
...
...
@@ -167,14 +175,14 @@ template<>
bool
GJKSolver
::
shapeIntersect
<
Capsule
,
Halfspace
>
(
const
Capsule
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
capsuleHalfspaceIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
...
...
@@ -184,14 +192,14 @@ template<>
bool
GJKSolver
::
shapeIntersect
<
Cylinder
,
Halfspace
>
(
const
Cylinder
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
cylinderHalfspaceIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
...
...
@@ -201,41 +209,49 @@ template<>
bool
GJKSolver
::
shapeIntersect
<
Cone
,
Halfspace
>
(
const
Cone
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
coneHalfspaceIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
SHAPE_INTERSECT_INVERTED
(
Halfspace
,
Cone
)
template
<
>
bool
GJKSolver
::
shapeIntersect
<
Halfspace
,
Halfspace
>
(
const
Halfspace
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
/*contact_points*/
,
FCL_REAL
*
/*penetration_depth*/
,
Vec3f
*
/*normal*/
)
const
bool
GJKSolver
::
shapeIntersect
<
Halfspace
,
Halfspace
>
(
const
Halfspace
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
FCL_REAL
&
distance
,
bool
,
Vec3f
*
/*contact_points*/
,
Vec3f
*
/*normal*/
)
const
{
Halfspace
s
;
Vec3f
p
,
d
;
FCL_REAL
depth
;
int
ret
;
return
details
::
halfspaceIntersect
(
s1
,
tf1
,
s2
,
tf2
,
p
,
d
,
s
,
depth
,
ret
);
bool
res
=
details
::
halfspaceIntersect
(
s1
,
tf1
,
s2
,
tf2
,
p
,
d
,
s
,
depth
,
ret
);
distance
=
-
depth
;
return
res
;
}
template
<
>
bool
GJKSolver
::
shapeIntersect
<
Plane
,
Halfspace
>
(
const
Plane
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
/*contact_points*/
,
FCL_REAL
*
/*penetration_depth*/
,
Vec3f
*
/*normal*/
)
const
bool
GJKSolver
::
shapeIntersect
<
Plane
,
Halfspace
>
(
const
Plane
&
s1
,
const
Transform3f
&
tf1
,
const
Halfspace
&
s2
,
const
Transform3f
&
tf2
,
FCL_REAL
&
distance
,
bool
,
Vec3f
*
/*contact_points*/
,
Vec3f
*
/*normal*/
)
const
{
Plane
pl
;
Vec3f
p
,
d
;
FCL_REAL
depth
;
int
ret
;
return
details
::
planeHalfspaceIntersect
(
s1
,
tf1
,
s2
,
tf2
,
pl
,
p
,
d
,
depth
,
ret
);
bool
res
=
details
::
planeHalfspaceIntersect
(
s1
,
tf1
,
s2
,
tf2
,
pl
,
p
,
d
,
depth
,
ret
);
distance
=
-
depth
;
return
res
;
}
SHAPE_INTERSECT_INVERTED
(
Halfspace
,
Plane
)
...
...
@@ -244,14 +260,14 @@ template<>
bool
GJKSolver
::
shapeIntersect
<
Sphere
,
Plane
>
(
const
Sphere
&
s1
,
const
Transform3f
&
tf1
,
const
Plane
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
spherePlaneIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
...
...
@@ -261,14 +277,14 @@ template<>
bool
GJKSolver
::
shapeIntersect
<
Box
,
Plane
>
(
const
Box
&
s1
,
const
Transform3f
&
tf1
,
const
Plane
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
boxPlaneIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
...
...
@@ -278,14 +294,14 @@ template<>
bool
GJKSolver
::
shapeIntersect
<
Capsule
,
Plane
>
(
const
Capsule
&
s1
,
const
Transform3f
&
tf1
,
const
Plane
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
capsulePlaneIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
...
...
@@ -295,14 +311,14 @@ template<>
bool
GJKSolver
::
shapeIntersect
<
Cylinder
,
Plane
>
(
const
Cylinder
&
s1
,
const
Transform3f
&
tf1
,
const
Plane
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
cylinderPlaneIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
...
...
@@ -312,25 +328,27 @@ template<>
bool
GJKSolver
::
shapeIntersect
<
Cone
,
Plane
>
(
const
Cone
&
s1
,
const
Transform3f
&
tf1
,
const
Plane
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
FCL_REAL
distance
;
Vec3f
p1
,
p2
;
Vec3f
p1
,
p2
,
n
;
bool
res
=
details
::
conePlaneIntersect
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
*
normal
);
*
contact_points
=
p1
;
*
penetration_depth
=
-
distance
;
(
s1
,
tf1
,
s2
,
tf2
,
distance
,
p1
,
p2
,
n
);
if
(
contact_points
)
*
contact_points
=
p1
;
if
(
normal
)
*
normal
=
n
;
return
res
;
}
SHAPE_INTERSECT_INVERTED
(
Plane
,
Cone
)
template
<
>
bool
GJKSolver
::
shapeIntersect
<
Plane
,
Plane
>
(
const
Plane
&
s1
,
const
Transform3f
&
tf1
,
const
Plane
&
s2
,
const
Transform3f
&
tf2
,
Vec3f
*
contact_points
,
FCL_REAL
*
penetration_depth
,
Vec3f
*
normal
)
const
bool
GJKSolver
::
shapeIntersect
<
Plane
,
Plane
>
(
const
Plane
&
s1
,
const
Transform3f
&
tf1
,
const
Plane
&
s2
,
const
Transform3f
&
tf2
,
FCL_REAL
&
distance
,
bool
,
Vec3f
*
contact_points
,
Vec3f
*
normal
)
const
{
return
details
::
planeIntersect
(
s1
,
tf1
,
s2
,
tf2
,
contact_points
,
penetration_depth
,
normal
);
return
details
::
planeIntersect
(
s1
,
tf1
,
s2
,
tf2
,
contact_points
,
&
distance
,
normal
);
}
...
...
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