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
5712c6a4
Commit
5712c6a4
authored
Aug 31, 2019
by
Joseph Mirabel
Browse files
[GJK] Put shapes in the same frame when possible.
parent
4cb0ed89
Changes
5
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/gjk.h
View file @
5712c6a4
...
...
@@ -77,13 +77,13 @@ struct MinkowskiDiff
MinkowskiDiff
()
:
getSupportFunc
(
NULL
)
{}
/// Set the two shapes,
/// assuming the relative transformation between them is identity.
void
set
(
const
ShapeBase
*
shape0
,
const
ShapeBase
*
shape1
);
void
set
(
const
Transform3f
&
tf0
,
const
Transform3f
&
tf1
)
{
oR1
=
tf0
.
getRotation
().
transpose
()
*
tf1
.
getRotation
();
ot1
=
tf0
.
getRotation
().
transpose
()
*
(
tf1
.
getTranslation
()
-
tf0
.
getTranslation
());
}
/// Set the two shapes, with a relative transformation.
void
set
(
const
ShapeBase
*
shape0
,
const
ShapeBase
*
shape1
,
const
Transform3f
&
tf0
,
const
Transform3f
&
tf1
);
/// @brief support function for shape0
inline
Vec3f
support0
(
const
Vec3f
&
d
,
bool
dIsNormalized
)
const
...
...
include/hpp/fcl/narrowphase/narrowphase.h
View file @
5712c6a4
...
...
@@ -62,8 +62,7 @@ namespace fcl
if
(
enable_cached_guess
)
guess
=
cached_guess
;
details
::
MinkowskiDiff
shape
;
shape
.
set
(
&
s1
,
&
s2
);
shape
.
set
(
tf1
,
tf2
);
shape
.
set
(
&
s1
,
&
s2
,
tf1
,
tf2
);
details
::
GJK
gjk
((
unsigned
int
)
gjk_max_iterations
,
gjk_tolerance
);
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
);
...
...
@@ -103,14 +102,18 @@ namespace fcl
Vec3f
&
p1
,
Vec3f
&
p2
,
Vec3f
&
normal
)
const
{
bool
col
;
TriangleP
tri
(
P1
,
P2
,
P3
);
// Express everything in frame 1
const
Transform3f
tf_1M2
(
tf1
.
inverseTimes
(
tf2
));
TriangleP
tri
(
tf_1M2
.
transform
(
P1
),
tf_1M2
.
transform
(
P2
),
tf_1M2
.
transform
(
P3
));
Vec3f
guess
(
1
,
0
,
0
);
if
(
enable_cached_guess
)
guess
=
cached_guess
;
details
::
MinkowskiDiff
shape
;
shape
.
set
(
&
s
,
&
tri
);
shape
.
set
(
tf1
,
tf2
);
details
::
GJK
gjk
((
unsigned
int
)
gjk_max_iterations
,
gjk_tolerance
);
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
);
...
...
@@ -170,8 +173,7 @@ namespace fcl
if
(
enable_cached_guess
)
guess
=
cached_guess
;
details
::
MinkowskiDiff
shape
;
shape
.
set
(
&
s1
,
&
s2
);
shape
.
set
(
tf1
,
tf2
);
shape
.
set
(
&
s1
,
&
s2
,
tf1
,
tf2
);
details
::
GJK
gjk
((
unsigned
int
)
gjk_max_iterations
,
gjk_tolerance
);
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
);
...
...
src/narrowphase/details.h
View file @
5712c6a4
...
...
@@ -2626,6 +2626,21 @@ namespace fcl {
return
true
;
}
/// See the prototype below
inline
FCL_REAL
computePenetration
(
const
Vec3f
&
P1
,
const
Vec3f
&
P2
,
const
Vec3f
&
P3
,
const
Vec3f
&
Q1
,
const
Vec3f
&
Q2
,
const
Vec3f
&
Q3
,
Vec3f
&
normal
)
{
Vec3f
u
((
P2
-
P1
).
cross
(
P3
-
P1
));
normal
=
u
.
normalized
();
FCL_REAL
depth1
((
P1
-
Q1
).
dot
(
normal
));
FCL_REAL
depth2
((
P1
-
Q2
).
dot
(
normal
));
FCL_REAL
depth3
((
P1
-
Q3
).
dot
(
normal
));
return
std
::
max
(
depth1
,
std
::
max
(
depth2
,
depth3
));
}
// Compute penetration distance and normal of two triangles in collision
// Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the
// minimal distance (Q1, Q2, Q3) should be translated along the normal so
...
...
@@ -2644,14 +2659,8 @@ namespace fcl {
Vec3f
globalQ1
(
tf2
.
transform
(
Q1
));
Vec3f
globalQ2
(
tf2
.
transform
(
Q2
));
Vec3f
globalQ3
(
tf2
.
transform
(
Q3
));
Vec3f
u
((
globalP2
-
globalP1
).
cross
(
globalP3
-
globalP1
));
normal
=
u
.
normalized
();
FCL_REAL
depth
;
FCL_REAL
depth1
((
globalP1
-
globalQ1
).
dot
(
normal
));
FCL_REAL
depth2
((
globalP1
-
globalQ2
).
dot
(
normal
));
FCL_REAL
depth3
((
globalP1
-
globalQ3
).
dot
(
normal
));
depth
=
std
::
max
(
depth1
,
std
::
max
(
depth2
,
depth3
));
return
depth
;
return
computePenetration
(
globalP1
,
globalP2
,
globalP3
,
globalQ1
,
globalQ2
,
globalQ3
,
normal
);
}
}
// details
}
// namespace fcl
...
...
src/narrowphase/gjk.cpp
View file @
5712c6a4
...
...
@@ -210,17 +210,21 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized)
#undef CALL_GET_SHAPE_SUPPORT
template
<
typename
Shape0
,
typename
Shape1
>
template
<
typename
Shape0
,
typename
Shape1
,
bool
TransformIsIdentity
>
void
getSupportTpl
(
const
Shape0
*
s0
,
const
Shape1
*
s1
,
const
Matrix3f
&
oR1
,
const
Vec3f
&
ot1
,
const
Vec3f
&
dir
,
Vec3f
&
support0
,
Vec3f
&
support1
)
{
getShapeSupport
(
s0
,
dir
,
support0
);
getShapeSupport
(
s1
,
-
oR1
.
transpose
()
*
dir
,
support1
);
support1
=
oR1
*
support1
+
ot1
;
if
(
TransformIsIdentity
)
getShapeSupport
(
s1
,
-
dir
,
support1
);
else
{
getShapeSupport
(
s1
,
-
oR1
.
transpose
()
*
dir
,
support1
);
support1
=
oR1
*
support1
+
ot1
;
}
}
template
<
typename
Shape0
,
typename
Shape1
>
template
<
typename
Shape0
,
typename
Shape1
,
bool
TransformIsIdentity
>
void
getSupportFuncTpl
(
const
MinkowskiDiff
&
md
,
const
Vec3f
&
dir
,
bool
dirIsNormalized
,
Vec3f
&
support0
,
Vec3f
&
support1
)
{
...
...
@@ -228,7 +232,7 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
bool
(
(
bool
)
shape_traits
<
Shape0
>::
NeedNormalizedDir
||
(
bool
)
shape_traits
<
Shape1
>::
NeedNormalizedDir
)
};
getSupportTpl
<
Shape0
,
Shape1
>
(
getSupportTpl
<
Shape0
,
Shape1
,
TransformIsIdentity
>
(
static_cast
<
const
Shape0
*>
(
md
.
shapes
[
0
]),
static_cast
<
const
Shape1
*>
(
md
.
shapes
[
1
]),
md
.
oR1
,
md
.
ot1
,
...
...
@@ -237,62 +241,91 @@ void getSupportFuncTpl (const MinkowskiDiff& md,
}
template
<
typename
Shape0
>
MinkowskiDiff
::
GetSupportFunction
makeGetSupportFunction1
(
const
ShapeBase
*
s1
)
MinkowskiDiff
::
GetSupportFunction
makeGetSupportFunction1
(
const
ShapeBase
*
s1
,
bool
identity
)
{
switch
(
s1
->
getNodeType
())
{
case
GEOM_TRIANGLE
:
return
getSupportFuncTpl
<
Shape0
,
TriangleP
>
;
if
(
identity
)
return
getSupportFuncTpl
<
Shape0
,
TriangleP
,
true
>
;
else
return
getSupportFuncTpl
<
Shape0
,
TriangleP
,
false
>
;
case
GEOM_BOX
:
return
getSupportFuncTpl
<
Shape0
,
Box
>
;
if
(
identity
)
return
getSupportFuncTpl
<
Shape0
,
Box
,
true
>
;
else
return
getSupportFuncTpl
<
Shape0
,
Box
,
false
>
;
case
GEOM_SPHERE
:
return
getSupportFuncTpl
<
Shape0
,
Sphere
>
;
if
(
identity
)
return
getSupportFuncTpl
<
Shape0
,
Sphere
,
true
>
;
else
return
getSupportFuncTpl
<
Shape0
,
Sphere
,
false
>
;
case
GEOM_CAPSULE
:
return
getSupportFuncTpl
<
Shape0
,
Capsule
>
;
if
(
identity
)
return
getSupportFuncTpl
<
Shape0
,
Capsule
,
true
>
;
else
return
getSupportFuncTpl
<
Shape0
,
Capsule
,
false
>
;
case
GEOM_CONE
:
return
getSupportFuncTpl
<
Shape0
,
Cone
>
;
if
(
identity
)
return
getSupportFuncTpl
<
Shape0
,
Cone
,
true
>
;
else
return
getSupportFuncTpl
<
Shape0
,
Cone
,
false
>
;
case
GEOM_CYLINDER
:
return
getSupportFuncTpl
<
Shape0
,
Cylinder
>
;
if
(
identity
)
return
getSupportFuncTpl
<
Shape0
,
Cylinder
,
true
>
;
else
return
getSupportFuncTpl
<
Shape0
,
Cylinder
,
false
>
;
case
GEOM_CONVEX
:
return
getSupportFuncTpl
<
Shape0
,
Convex
>
;
if
(
identity
)
return
getSupportFuncTpl
<
Shape0
,
Convex
,
true
>
;
else
return
getSupportFuncTpl
<
Shape0
,
Convex
,
false
>
;
default:
throw
std
::
logic_error
(
"Unsupported geometric shape"
);
}
}
void
MinkowskiDiff
::
s
et
(
const
ShapeBase
*
s
hape
0
,
const
ShapeBase
*
s
hape1
)
MinkowskiDiff
::
G
et
SupportFunction
makeGetSupportFunction0
(
const
ShapeBase
*
s0
,
const
ShapeBase
*
s
1
,
bool
identity
)
{
shapes
[
0
]
=
shape0
;
shapes
[
1
]
=
shape1
;
switch
(
shape0
->
getNodeType
())
switch
(
s0
->
getNodeType
())
{
case
GEOM_TRIANGLE
:
g
et
SupportFunc
=
makeGetSupportFunction1
<
TriangleP
>
(
s
hape1
);
r
et
urn
makeGetSupportFunction1
<
TriangleP
>
(
s
1
,
identity
);
break
;
case
GEOM_BOX
:
g
et
SupportFunc
=
makeGetSupportFunction1
<
Box
>
(
s
hape1
);
r
et
urn
makeGetSupportFunction1
<
Box
>
(
s
1
,
identity
);
break
;
case
GEOM_SPHERE
:
g
et
SupportFunc
=
makeGetSupportFunction1
<
Sphere
>
(
s
hape1
);
r
et
urn
makeGetSupportFunction1
<
Sphere
>
(
s
1
,
identity
);
break
;
case
GEOM_CAPSULE
:
g
et
SupportFunc
=
makeGetSupportFunction1
<
Capsule
>
(
s
hape1
);
r
et
urn
makeGetSupportFunction1
<
Capsule
>
(
s
1
,
identity
);
break
;
case
GEOM_CONE
:
g
et
SupportFunc
=
makeGetSupportFunction1
<
Cone
>
(
s
hape1
);
r
et
urn
makeGetSupportFunction1
<
Cone
>
(
s
1
,
identity
);
break
;
case
GEOM_CYLINDER
:
g
et
SupportFunc
=
makeGetSupportFunction1
<
Cylinder
>
(
s
hape1
);
r
et
urn
makeGetSupportFunction1
<
Cylinder
>
(
s
1
,
identity
);
break
;
case
GEOM_CONVEX
:
g
et
SupportFunc
=
makeGetSupportFunction1
<
Convex
>
(
s
hape1
);
r
et
urn
makeGetSupportFunction1
<
Convex
>
(
s
1
,
identity
);
break
;
default:
throw
std
::
logic_error
(
"Unsupported geometric shape"
);
}
}
void
MinkowskiDiff
::
set
(
const
ShapeBase
*
shape0
,
const
ShapeBase
*
shape1
,
const
Transform3f
&
tf0
,
const
Transform3f
&
tf1
)
{
shapes
[
0
]
=
shape0
;
shapes
[
1
]
=
shape1
;
oR1
=
tf0
.
getRotation
().
transpose
()
*
tf1
.
getRotation
();
ot1
=
tf0
.
getRotation
().
transpose
()
*
(
tf1
.
getTranslation
()
-
tf0
.
getTranslation
());
bool
identity
=
(
oR1
.
isIdentity
()
&&
ot1
.
isZero
());
getSupportFunc
=
makeGetSupportFunction0
(
shape0
,
shape1
,
identity
);
}
void
MinkowskiDiff
::
set
(
const
ShapeBase
*
shape0
,
const
ShapeBase
*
shape1
)
{
shapes
[
0
]
=
shape0
;
shapes
[
1
]
=
shape1
;
oR1
.
setIdentity
();
ot1
.
setZero
();
getSupportFunc
=
makeGetSupportFunction0
(
shape0
,
shape1
,
true
);
}
void
GJK
::
initialize
()
{
nfree
=
0
;
...
...
src/narrowphase/narrowphase.cpp
View file @
5712c6a4
...
...
@@ -625,20 +625,16 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
const
TriangleP
&
s2
,
const
Transform3f
&
tf2
,
FCL_REAL
&
dist
,
Vec3f
&
p1
,
Vec3f
&
p2
,
Vec3f
&
normal
)
const
{
const
TriangleP
t1
(
tf1
.
transform
(
s1
.
a
),
tf1
.
transform
(
s1
.
b
),
tf1
.
transform
(
s1
.
c
)),
t2
(
tf2
.
transform
(
s2
.
a
),
tf2
.
transform
(
s2
.
b
),
tf2
.
transform
(
s2
.
c
));
Vec3f
guess
(
1
,
0
,
0
);
if
(
enable_cached_guess
)
guess
=
cached_guess
;
bool
enable_penetration
(
true
);
details
::
MinkowskiDiff
shape
;
shape
.
set
(
&
s1
,
&
s2
);
shape
.
set
(
tf1
,
tf2
);
const
Vec3f
&
P1
(
s1
.
a
);
const
Vec3f
&
P2
(
s1
.
b
);
const
Vec3f
&
P3
(
s1
.
c
);
const
Vec3f
&
Q1
(
s2
.
a
);
const
Vec3f
&
Q2
(
s2
.
b
);
const
Vec3f
&
Q3
(
s2
.
c
);
shape
.
set
(
&
t1
,
&
t2
);
details
::
GJK
gjk
((
unsigned
int
)
gjk_max_iterations
,
gjk_tolerance
);
details
::
GJK
::
Status
gjk_status
=
gjk
.
evaluate
(
shape
,
-
guess
);
...
...
@@ -653,19 +649,14 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
// (i.e. an object face normal is colinear to gjk.ray
// assert (dist == (w0 - w1).norm());
dist
=
gjk
.
distance
;
p1
=
tf1
.
transform
(
p1
);
p2
=
tf1
.
transform
(
p2
);
return
true
;
}
else
if
(
gjk_status
==
details
::
GJK
::
Inside
)
{
p1
=
tf1
.
transform
(
p1
);
p2
=
tf1
.
transform
(
p2
);
if
(
enable_penetration
)
{
FCL_REAL
penetrationDepth
=
details
::
computePenetration
(
P1
,
P2
,
P3
,
Q1
,
Q2
,
Q3
,
tf1
,
tf2
,
normal
);
(
t1
.
a
,
t1
.
b
,
t1
.
c
,
t2
.
a
,
t2
.
b
,
t2
.
c
,
normal
);
dist
=
-
penetrationDepth
;
assert
(
dist
<=
1e-6
);
// GJK says Inside when below GJK.tolerance. So non intersecting
...
...
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