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
d691c1a9
Commit
d691c1a9
authored
Aug 28, 2019
by
Joseph Mirabel
Browse files
Prepare update handling of GJK support function.
parent
baf0bebb
Changes
4
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/gjk.h
View file @
d691c1a9
...
...
@@ -68,6 +68,8 @@ struct MinkowskiDiff
MinkowskiDiff
()
{
}
void
set
(
const
ShapeBase
*
shape0
,
const
ShapeBase
*
shape1
);
/// @brief support function for shape0
inline
Vec3f
support0
(
const
Vec3f
&
d
)
const
{
...
...
@@ -96,7 +98,6 @@ struct MinkowskiDiff
}
};
/// @brief class for GJK algorithm
///
/// \note The computations are performed in the frame of the first shape.
...
...
include/hpp/fcl/narrowphase/narrowphase.h
View file @
d691c1a9
...
...
@@ -62,8 +62,7 @@ namespace fcl
if
(
enable_cached_guess
)
guess
=
cached_guess
;
details
::
MinkowskiDiff
shape
;
shape
.
shapes
[
0
]
=
&
s1
;
shape
.
shapes
[
1
]
=
&
s2
;
shape
.
set
(
&
s1
,
&
s2
);
shape
.
toshape1
=
tf2
.
getRotation
().
transpose
()
*
tf1
.
getRotation
();
shape
.
toshape0
=
tf1
.
inverseTimes
(
tf2
);
...
...
@@ -115,8 +114,7 @@ namespace fcl
if
(
enable_cached_guess
)
guess
=
cached_guess
;
details
::
MinkowskiDiff
shape
;
shape
.
shapes
[
0
]
=
&
s
;
shape
.
shapes
[
1
]
=
&
tri
;
shape
.
set
(
&
s
,
&
tri
);
shape
.
toshape1
=
tf2
.
getRotation
().
transpose
()
*
tf1
.
getRotation
();
shape
.
toshape0
=
tf1
.
inverseTimes
(
tf2
);
...
...
@@ -183,8 +181,7 @@ namespace fcl
if
(
enable_cached_guess
)
guess
=
cached_guess
;
details
::
MinkowskiDiff
shape
;
shape
.
shapes
[
0
]
=
&
s1
;
shape
.
shapes
[
1
]
=
&
s2
;
shape
.
set
(
&
s1
,
&
s2
);
shape
.
toshape1
=
tf2
.
getRotation
().
transpose
()
*
tf1
.
getRotation
();
shape
.
toshape0
=
tf1
.
inverseTimes
(
tf2
);
...
...
src/narrowphase/gjk.cpp
View file @
d691c1a9
...
...
@@ -46,129 +46,156 @@ namespace fcl
namespace
details
{
void
getShapeSupport
(
const
TriangleP
*
triangle
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
FCL_REAL
dota
=
dir
.
dot
(
triangle
->
a
);
FCL_REAL
dotb
=
dir
.
dot
(
triangle
->
b
);
FCL_REAL
dotc
=
dir
.
dot
(
triangle
->
c
);
if
(
dota
>
dotb
)
{
if
(
dotc
>
dota
)
support
=
triangle
->
c
;
else
support
=
triangle
->
a
;
}
else
{
if
(
dotc
>
dotb
)
support
=
triangle
->
c
;
else
support
=
triangle
->
b
;
}
}
inline
void
getShapeSupport
(
const
Box
*
box
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
support
.
noalias
()
=
(
dir
.
array
()
>
0
).
select
(
box
->
halfSide
,
-
box
->
halfSide
);
}
inline
void
getShapeSupport
(
const
Sphere
*
sphere
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
support
=
dir
*
sphere
->
radius
;
}
inline
void
getShapeSupport
(
const
Capsule
*
capsule
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
support
=
capsule
->
radius
*
dir
;
if
(
dir
[
2
]
>
0
)
support
[
2
]
+=
capsule
->
lz
/
2
;
else
support
[
2
]
-=
capsule
->
lz
/
2
;
}
void
getShapeSupport
(
const
Cone
*
cone
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
// TODO (Joseph Mirabel)
// this assumes that the cone radius is, for -0.5 < z < 0.5:
// (lz/2 - z) * radius / lz
//
// I did not change the code myself. However, I think it should be revised.
// 1. It can be optimized.
// 2. I am not sure this is what conePlaneIntersect and coneHalfspaceIntersect
// assumes...
FCL_REAL
zdist
=
dir
[
0
]
*
dir
[
0
]
+
dir
[
1
]
*
dir
[
1
];
FCL_REAL
len
=
zdist
+
dir
[
2
]
*
dir
[
2
];
zdist
=
std
::
sqrt
(
zdist
);
len
=
std
::
sqrt
(
len
);
FCL_REAL
half_h
=
cone
->
lz
*
0.5
;
FCL_REAL
radius
=
cone
->
radius
;
FCL_REAL
sin_a
=
radius
/
std
::
sqrt
(
radius
*
radius
+
4
*
half_h
*
half_h
);
if
(
dir
[
2
]
>
len
*
sin_a
)
support
=
Vec3f
(
0
,
0
,
half_h
);
else
if
(
zdist
>
0
)
{
FCL_REAL
rad
=
radius
/
zdist
;
support
=
Vec3f
(
rad
*
dir
[
0
],
rad
*
dir
[
1
],
-
half_h
);
}
else
support
=
Vec3f
(
0
,
0
,
-
half_h
);
}
void
getShapeSupport
(
const
Cylinder
*
cylinder
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
static
const
FCL_REAL
eps
(
sqrt
(
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()));
FCL_REAL
zdist
=
std
::
sqrt
(
dir
[
0
]
*
dir
[
0
]
+
dir
[
1
]
*
dir
[
1
]);
FCL_REAL
half_h
=
cylinder
->
lz
*
0.5
;
if
(
zdist
==
0.0
)
support
=
Vec3f
(
0
,
0
,
(
dir
[
2
]
>
0
)
?
half_h
:-
half_h
);
else
{
FCL_REAL
d
=
cylinder
->
radius
/
zdist
;
FCL_REAL
z
(
0.
);
if
(
dir
[
2
]
>
eps
)
z
=
half_h
;
else
if
(
dir
[
2
]
<
-
eps
)
z
=
-
half_h
;
support
<<
d
*
dir
.
head
<
2
>
(),
z
;
}
assert
(
fabs
(
support
[
0
]
*
dir
[
1
]
-
support
[
1
]
*
dir
[
0
])
<
eps
);
}
void
getShapeSupport
(
const
Convex
*
convex
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
FCL_REAL
maxdot
=
-
std
::
numeric_limits
<
FCL_REAL
>::
max
();
Vec3f
*
curp
=
convex
->
points
;
for
(
int
i
=
0
;
i
<
convex
->
num_points
;
++
i
,
curp
+=
1
)
{
FCL_REAL
dot
=
dir
.
dot
(
*
curp
);
if
(
dot
>
maxdot
)
{
support
=
*
curp
;
maxdot
=
dot
;
}
}
}
Vec3f
getSupport
(
const
ShapeBase
*
shape
,
const
Vec3f
&
dir
)
{
FCL_REAL
eps
(
sqrt
(
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()))
;
Vec3f
support
;
switch
(
shape
->
getNodeType
())
{
case
GEOM_TRIANGLE
:
{
const
TriangleP
*
triangle
=
static_cast
<
const
TriangleP
*>
(
shape
);
FCL_REAL
dota
=
dir
.
dot
(
triangle
->
a
);
FCL_REAL
dotb
=
dir
.
dot
(
triangle
->
b
);
FCL_REAL
dotc
=
dir
.
dot
(
triangle
->
c
);
if
(
dota
>
dotb
)
{
if
(
dotc
>
dota
)
return
triangle
->
c
;
else
return
triangle
->
a
;
}
else
{
if
(
dotc
>
dotb
)
return
triangle
->
c
;
else
return
triangle
->
b
;
}
}
getShapeSupport
(
static_cast
<
const
TriangleP
*>
(
shape
),
dir
,
support
);
break
;
case
GEOM_BOX
:
{
const
Box
*
box
=
static_cast
<
const
Box
*>
(
shape
);
return
(
dir
.
array
()
>
0
).
select
(
box
->
halfSide
,
-
box
->
halfSide
);
}
getShapeSupport
(
static_cast
<
const
Box
*>
(
shape
),
dir
,
support
);
break
;
case
GEOM_SPHERE
:
{
const
Sphere
*
sphere
=
static_cast
<
const
Sphere
*>
(
shape
);
return
dir
*
sphere
->
radius
;
}
getShapeSupport
(
static_cast
<
const
Sphere
*>
(
shape
),
dir
,
support
);
break
;
case
GEOM_CAPSULE
:
{
const
Capsule
*
capsule
=
static_cast
<
const
Capsule
*>
(
shape
);
FCL_REAL
half_h
=
capsule
->
lz
*
0.5
;
Vec3f
pos1
(
0
,
0
,
half_h
);
Vec3f
pos2
(
0
,
0
,
-
half_h
);
Vec3f
v
=
dir
*
capsule
->
radius
;
pos1
+=
v
;
pos2
+=
v
;
if
(
dir
.
dot
(
pos1
)
>
dir
.
dot
(
pos2
))
return
pos1
;
else
return
pos2
;
}
getShapeSupport
(
static_cast
<
const
Capsule
*>
(
shape
),
dir
,
support
);
break
;
case
GEOM_CONE
:
{
const
Cone
*
cone
=
static_cast
<
const
Cone
*>
(
shape
);
FCL_REAL
zdist
=
dir
[
0
]
*
dir
[
0
]
+
dir
[
1
]
*
dir
[
1
];
FCL_REAL
len
=
zdist
+
dir
[
2
]
*
dir
[
2
];
zdist
=
std
::
sqrt
(
zdist
);
len
=
std
::
sqrt
(
len
);
FCL_REAL
half_h
=
cone
->
lz
*
0.5
;
FCL_REAL
radius
=
cone
->
radius
;
FCL_REAL
sin_a
=
radius
/
std
::
sqrt
(
radius
*
radius
+
4
*
half_h
*
half_h
);
if
(
dir
[
2
]
>
len
*
sin_a
)
return
Vec3f
(
0
,
0
,
half_h
);
else
if
(
zdist
>
0
)
{
FCL_REAL
rad
=
radius
/
zdist
;
return
Vec3f
(
rad
*
dir
[
0
],
rad
*
dir
[
1
],
-
half_h
);
}
else
return
Vec3f
(
0
,
0
,
-
half_h
);
}
getShapeSupport
(
static_cast
<
const
Cone
*>
(
shape
),
dir
,
support
);
break
;
case
GEOM_CYLINDER
:
{
const
Cylinder
*
cylinder
=
static_cast
<
const
Cylinder
*>
(
shape
);
FCL_REAL
zdist
=
std
::
sqrt
(
dir
[
0
]
*
dir
[
0
]
+
dir
[
1
]
*
dir
[
1
]);
FCL_REAL
half_h
=
cylinder
->
lz
*
0.5
;
Vec3f
res
;
if
(
zdist
==
0.0
)
{
res
=
Vec3f
(
0
,
0
,
(
dir
[
2
]
>
0
)
?
half_h
:-
half_h
);
}
else
{
FCL_REAL
d
=
cylinder
->
radius
/
zdist
;
FCL_REAL
z
(
0.
);
if
(
dir
[
2
]
>
eps
)
z
=
half_h
;
else
if
(
dir
[
2
]
<
-
eps
)
z
=
-
half_h
;
res
=
Vec3f
(
d
*
dir
[
0
],
d
*
dir
[
1
],
z
);
}
assert
(
fabs
(
res
[
0
]
*
dir
[
1
]
-
res
[
1
]
*
dir
[
0
])
<
eps
);
return
res
;
}
getShapeSupport
(
static_cast
<
const
Cylinder
*>
(
shape
),
dir
,
support
);
break
;
case
GEOM_CONVEX
:
{
const
Convex
*
convex
=
static_cast
<
const
Convex
*>
(
shape
);
FCL_REAL
maxdot
=
-
std
::
numeric_limits
<
FCL_REAL
>::
max
();
Vec3f
*
curp
=
convex
->
points
;
Vec3f
bestv
(
0
,
0
,
0
);
for
(
int
i
=
0
;
i
<
convex
->
num_points
;
++
i
,
curp
+=
1
)
{
FCL_REAL
dot
=
dir
.
dot
(
*
curp
);
if
(
dot
>
maxdot
)
{
bestv
=
*
curp
;
maxdot
=
dot
;
}
}
return
bestv
;
}
getShapeSupport
(
static_cast
<
const
Convex
*>
(
shape
),
dir
,
support
);
break
;
case
GEOM_PLANE
:
break
;
case
GEOM_HALFSPACE
:
default:
;
// nothing
}
return
Vec3f
(
0
,
0
,
0
);
return
support
;
}
template
<
typename
Shape0
,
typename
Shape1
>
void
getSupportTpl
(
const
Shape0
*
s0
,
const
Shape1
*
s1
,
const
Matrix3f
&
oM1
,
const
Vec3f
&
ot1
,
const
Vec3f
&
dir
,
Vec3f
&
support
)
{
getShapeSupport
(
s0
,
dir
,
support
);
Vec3f
support1
;
getShapeSupport
(
s1
,
-
oM1
.
transpose
()
*
dir
,
support1
);
support
.
noalias
()
-=
oM1
*
support1
+
ot1
;
}
void
MinkowskiDiff
::
set
(
const
ShapeBase
*
shape0
,
const
ShapeBase
*
shape1
)
{
shapes
[
0
]
=
shape0
;
shapes
[
1
]
=
shape1
;
}
void
GJK
::
initialize
()
...
...
@@ -181,13 +208,11 @@ void GJK::initialize()
simplex
=
NULL
;
}
Vec3f
GJK
::
getGuessFromSimplex
()
const
{
return
ray
;
}
GJK
::
Status
GJK
::
evaluate
(
const
MinkowskiDiff
&
shape_
,
const
Vec3f
&
guess
)
{
size_t
iterations
=
0
;
...
...
@@ -209,7 +234,6 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
simplices
[
0
].
rank
=
0
;
ray
=
guess
;
// appendVertex(simplices[0], (ray.squaredNorm() > 0) ? -ray : Vec3f(1, 0, 0));
if
(
ray
.
squaredNorm
()
>
0
)
appendVertex
(
simplices
[
0
],
-
ray
);
else
appendVertex
(
simplices
[
0
],
Vec3f
(
1
,
0
,
0
));
simplices
[
0
].
coefficient
[
0
]
=
1
;
...
...
@@ -390,7 +414,6 @@ bool GJK::encloseOrigin()
return
false
;
}
void
EPA
::
initialize
()
{
sv_store
=
new
SimplexV
[
max_vertex_num
];
...
...
src/narrowphase/narrowphase.cpp
View file @
d691c1a9
...
...
@@ -630,8 +630,7 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
bool
enable_penetration
(
true
);
details
::
MinkowskiDiff
shape
;
shape
.
shapes
[
0
]
=
&
s1
;
shape
.
shapes
[
1
]
=
&
s2
;
shape
.
set
(
&
s1
,
&
s2
);
shape
.
toshape1
=
tf2
.
getRotation
().
transpose
()
*
tf1
.
getRotation
();
shape
.
toshape0
=
tf1
.
inverseTimes
(
tf2
);
...
...
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