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
6a1930cd
Commit
6a1930cd
authored
Dec 13, 2019
by
rstrudel
Browse files
Rewrite Capsule/Capsule distance function
parent
126cd3d7
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/distance_capsule_capsule.cpp
View file @
6a1930cd
...
...
@@ -12,6 +12,8 @@
#include
<hpp/fcl/shape/geometric_shapes.h>
#include
<../src/distance_func_matrix.h>
#define CLAMP(value, l, u) fmax(fmin(value, u), l)
// Note that partial specialization of template functions is not allowed.
// Therefore, two implementations with the default narrow phase solvers are
// provided. If another narrow phase solver were to be used, the default
...
...
@@ -25,299 +27,121 @@ namespace hpp
namespace
fcl
{
class
GJKSolver
;
// Compute the distance between C1 and C2 by computing the distance
// between the two segments supporting the capsules.
// Match algorithm of Real-Time Collision Detection, Christer Ericson - Closest Point of Two Line Segments
template
<
>
FCL_REAL
ShapeShapeDistance
<
Capsule
,
Capsule
>
(
const
CollisionGeometry
*
o1
,
const
Transform3f
&
tf1
,
FCL_REAL
ShapeShapeDistance
<
Capsule
,
Capsule
>
(
const
CollisionGeometry
*
o1
,
const
Transform3f
&
tf1
,
const
CollisionGeometry
*
o2
,
const
Transform3f
&
tf2
,
const
GJKSolver
*
,
const
DistanceRequest
&
request
,
DistanceResult
&
result
)
{
const
Capsule
*
c1
=
static_cast
<
const
Capsule
*>
(
o1
);
const
Capsule
*
c2
=
static_cast
<
const
Capsule
*>
(
o2
);
const
Capsule
*
capsule1
=
static_cast
<
const
Capsule
*>
(
o1
);
const
Capsule
*
capsule2
=
static_cast
<
const
Capsule
*>
(
o2
);
FCL_REAL
EPSILON
=
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()
*
100
;
// We assume that capsules are centered at the origin.
const
fcl
::
Vec3f
&
c
enter
1
=
tf1
.
getTranslation
();
const
fcl
::
Vec3f
&
c
enter
2
=
tf2
.
getTranslation
();
const
fcl
::
Vec3f
&
c1
=
tf1
.
getTranslation
();
const
fcl
::
Vec3f
&
c2
=
tf2
.
getTranslation
();
// We assume that capsules are oriented along z-axis.
Matrix3f
::
ConstColXpr
direction1
=
tf1
.
getRotation
().
col
(
2
);
Matrix3f
::
ConstColXpr
direction2
=
tf2
.
getRotation
().
col
(
2
);
FCL_REAL
halfLength1
=
c1
->
halfLength
;
FCL_REAL
halfLength2
=
c2
->
halfLength
;
FCL_REAL
halfLength1
=
capsule1
->
halfLength
;
FCL_REAL
halfLength2
=
capsule2
->
halfLength
;
FCL_REAL
radius1
=
capsule1
->
radius
;
FCL_REAL
radius2
=
capsule2
->
radius
;
// direction of capsules
// ||d1|| = 2 * halfLength1
// Matrix3f::ConstColXpr d1 = 2 * halfLength1 * tf1.getRotation().col(2);
// Matrix3f::ConstColXpr d2 = 2 * halfLength2 * tf2.getRotation().col(2);
const
fcl
::
Vec3f
&
d1
=
2
*
halfLength1
*
tf1
.
getRotation
().
col
(
2
);
const
fcl
::
Vec3f
&
d2
=
2
*
halfLength2
*
tf2
.
getRotation
().
col
(
2
);
Vec3f
diff
=
center1
-
center2
;
FCL_REAL
a01
=
-
direction1
.
dot
(
direction2
);
FCL_REAL
b0
=
diff
.
dot
(
direction1
);
FCL_REAL
b1
=
-
diff
.
dot
(
direction2
);
FCL_REAL
c
=
diff
.
squaredNorm
();
FCL_REAL
det
=
fabs
(
1.0
-
a01
*
a01
);
FCL_REAL
s1
,
s2
,
sqrDist
,
extDet0
,
extDet1
,
tmpS0
,
tmpS1
;
FCL_REAL
epsilon
=
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()
*
100
;
// Starting point of the segments
// p1 + d1 is the end point of the segment
const
fcl
::
Vec3f
&
p1
=
c1
-
d1
/
2
;
const
fcl
::
Vec3f
&
p2
=
c2
-
d2
/
2
;
const
fcl
::
Vec3f
&
r
=
p1
-
p2
;
FCL_REAL
a
=
d1
.
dot
(
d1
);
FCL_REAL
b
=
d1
.
dot
(
d2
);
FCL_REAL
c
=
d1
.
dot
(
r
);
FCL_REAL
e
=
d2
.
dot
(
d2
);
FCL_REAL
f
=
d2
.
dot
(
r
);
// S1 is parametrized by the equation p1 + s * d1
// S2 is parametrized by the equation p2 + t * d2
FCL_REAL
s
=
0.0
;
FCL_REAL
t
=
0.0
;
if
(
det
>=
epsilon
)
{
// Segments are not parallel.
s1
=
a01
*
b1
-
b0
;
s2
=
a01
*
b0
-
b1
;
extDet0
=
halfLength1
*
det
;
extDet1
=
halfLength2
*
det
;
if
(
s1
>=
-
extDet0
)
{
if
(
s1
<=
extDet0
)
{
if
(
s2
>=
-
extDet1
)
{
if
(
s2
<=
extDet1
)
{
// region 0 (interior)
// Minimum at interior points of segments.
FCL_REAL
invDet
=
(
1.0
)
/
det
;
s1
*=
invDet
;
s2
*=
invDet
;
sqrDist
=
s1
*
(
s1
+
a01
*
s2
+
2.0
*
b0
)
+
s2
*
(
a01
*
s1
+
s2
+
2.0
*
b1
)
+
c
;
}
else
{
// region 3 (side)
s2
=
halfLength2
;
tmpS0
=
-
(
a01
*
s2
+
b0
);
if
(
tmpS0
<
-
halfLength1
)
{
s1
=
-
halfLength1
;
sqrDist
=
s1
*
(
s1
-
2.0
*
tmpS0
)
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
if
(
tmpS0
<=
halfLength1
)
{
s1
=
tmpS0
;
sqrDist
=
-
s1
*
s1
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
{
s1
=
halfLength1
;
sqrDist
=
s1
*
(
s1
-
2.0
*
tmpS0
)
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
}
}
else
{
// region 7 (side)
s2
=
-
halfLength2
;
tmpS0
=
-
(
a01
*
s2
+
b0
);
if
(
tmpS0
<
-
halfLength1
)
{
s1
=
-
halfLength1
;
sqrDist
=
s1
*
(
s1
-
2.0
*
tmpS0
)
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
if
(
tmpS0
<=
halfLength1
)
{
s1
=
tmpS0
;
sqrDist
=
-
s1
*
s1
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
{
s1
=
halfLength1
;
sqrDist
=
s1
*
(
s1
-
2.0
*
tmpS0
)
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
}
}
else
{
if
(
s2
>=
-
extDet1
)
{
if
(
s2
<=
extDet1
)
{
// region 1 (side)
s1
=
halfLength1
;
tmpS1
=
-
(
a01
*
s1
+
b1
);
if
(
tmpS1
<
-
halfLength2
)
{
s2
=
-
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
if
(
tmpS1
<=
halfLength2
)
{
s2
=
tmpS1
;
sqrDist
=
-
s2
*
s2
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
{
s2
=
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
}
else
{
// region 2 (corner)
s2
=
halfLength2
;
tmpS0
=
-
(
a01
*
s2
+
b0
);
if
(
tmpS0
<
-
halfLength1
)
{
s1
=
-
halfLength1
;
sqrDist
=
s1
*
(
s1
-
2.0
*
tmpS0
)
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
if
(
tmpS0
<=
halfLength1
)
{
s1
=
tmpS0
;
sqrDist
=
-
s1
*
s1
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
{
s1
=
halfLength1
;
tmpS1
=
-
(
a01
*
s1
+
b1
);
if
(
tmpS1
<
-
halfLength2
)
{
s2
=
-
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
if
(
tmpS1
<=
halfLength2
)
{
s2
=
tmpS1
;
sqrDist
=
-
s2
*
s2
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
{
s2
=
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
}
}
}
else
{
// region 8 (corner)
s2
=
-
halfLength2
;
tmpS0
=
-
(
a01
*
s2
+
b0
);
if
(
tmpS0
<
-
halfLength1
)
{
s1
=
-
halfLength1
;
sqrDist
=
s1
*
(
s1
-
2.0
*
tmpS0
)
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
if
(
tmpS0
<=
halfLength1
)
{
s1
=
tmpS0
;
sqrDist
=
-
s1
*
s1
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
{
s1
=
halfLength1
;
tmpS1
=
-
(
a01
*
s1
+
b1
);
if
(
tmpS1
>
halfLength2
)
{
s2
=
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
if
(
tmpS1
>=
-
halfLength2
)
{
s2
=
tmpS1
;
sqrDist
=
-
s2
*
s2
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
{
s2
=
-
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
}
}
}
}
else
{
if
(
s2
>=
-
extDet1
)
{
if
(
s2
<=
extDet1
)
{
// region 5 (side)
s1
=
-
halfLength1
;
tmpS1
=
-
(
a01
*
s1
+
b1
);
if
(
tmpS1
<
-
halfLength2
)
{
s2
=
-
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
if
(
tmpS1
<=
halfLength2
)
{
s2
=
tmpS1
;
sqrDist
=
-
s2
*
s2
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
{
s2
=
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
}
else
{
// region 4 (corner)
s2
=
halfLength2
;
tmpS0
=
-
(
a01
*
s2
+
b0
);
if
(
tmpS0
>
halfLength1
)
{
s1
=
halfLength1
;
sqrDist
=
s1
*
(
s1
-
2.0
*
tmpS0
)
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
if
(
tmpS0
>=
-
halfLength1
)
{
s1
=
tmpS0
;
sqrDist
=
-
s1
*
s1
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
{
s1
=
-
halfLength1
;
tmpS1
=
-
(
a01
*
s1
+
b1
);
if
(
tmpS1
<
-
halfLength2
)
{
s2
=
-
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
if
(
tmpS1
<=
halfLength2
)
{
s2
=
tmpS1
;
sqrDist
=
-
s2
*
s2
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
{
s2
=
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
}
}
}
else
{
// region 6 (corner)
s2
=
-
halfLength2
;
tmpS0
=
-
(
a01
*
s2
+
b0
);
if
(
tmpS0
>
halfLength1
)
{
s1
=
halfLength1
;
sqrDist
=
s1
*
(
s1
-
2.0
*
tmpS0
)
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
if
(
tmpS0
>=
-
halfLength1
)
{
s1
=
tmpS0
;
sqrDist
=
-
s1
*
s1
+
s2
*
(
s2
+
2.0
*
b1
)
+
c
;
}
else
{
s1
=
-
halfLength1
;
tmpS1
=
-
(
a01
*
s1
+
b1
);
if
(
tmpS1
<
-
halfLength2
)
{
s2
=
-
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
if
(
tmpS1
<=
halfLength2
)
{
s2
=
tmpS1
;
sqrDist
=
-
s2
*
s2
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
else
{
s2
=
halfLength2
;
sqrDist
=
s2
*
(
s2
-
2.0
*
tmpS1
)
+
s1
*
(
s1
+
2.0
*
b0
)
+
c
;
}
}
}
if
(
a
<=
EPSILON
&&
e
<=
EPSILON
)
{
// Check if the segments degenerate into points
s
=
t
=
0.0
;
FCL_REAL
distance
=
(
p1
-
p2
).
norm
();
Vec3f
normal
=
(
p1
-
p2
)
/
distance
;
distance
=
distance
-
(
radius1
+
radius2
);
result
.
min_distance
=
distance
;
if
(
request
.
enable_nearest_points
)
{
result
.
nearest_points
[
0
]
=
p1
-
radius1
*
normal
;
result
.
nearest_points
[
1
]
=
p2
+
radius2
*
normal
;
}
return
distance
;
}
else
if
(
a
<=
EPSILON
)
{
// First segment is degenerated
s
=
0.0
;
t
=
CLAMP
(
f
/
e
,
0.0
,
1.0
);
}
else
{
// The segments are parallel. The average b0 term is designed to
// ensure symmetry of the function. That is, dist(seg0,seg1) and
// dist(seg1,seg0) should produce the same number.
FCL_REAL
e0pe1
=
halfLength1
+
halfLength2
;
FCL_REAL
sign
=
(
a01
>
0.0
?
-
1.0
:
1.0
);
FCL_REAL
b0Avr
=
(
0.5
)
*
(
b0
-
sign
*
b1
);
FCL_REAL
lambda
=
-
b0Avr
;
if
(
lambda
<
-
e0pe1
)
{
lambda
=
-
e0pe1
;
else
if
(
e
<=
EPSILON
)
{
// Second segment is degenerated
s
=
CLAMP
(
-
c
/
a
,
0.0
,
1.0
);
t
=
0.0
;
}
else
{
// Always non-negative, equal 0 if the segments are parallel
FCL_REAL
denom
=
a
*
e
-
b
*
b
;
if
(
denom
!=
0.0
)
{
s
=
CLAMP
((
b
*
f
-
c
*
e
)
/
denom
,
0.0
,
1.0
);
}
else
if
(
lambda
>
e0pe1
)
{
lambda
=
e0pe1
;
else
{
s
=
0.0
;
}
s2
=
-
sign
*
lambda
*
halfLength2
/
e0pe1
;
s1
=
lambda
+
sign
*
s2
;
sqrDist
=
lambda
*
(
lambda
+
2.0
*
b0Avr
)
+
c
;
t
=
(
b
*
s
+
f
)
/
e
;
if
(
t
<
0.0
)
{
t
=
0.0
;
s
=
CLAMP
(
-
c
/
a
,
0.0
,
1.0
);
}
else
if
(
t
>
1.0
)
{
t
=
1.0
;
s
=
CLAMP
((
b
-
c
)
/
a
,
0.0
,
1.0
);
}
}
Vec3f
closestOnSegment1
=
center1
+
s1
*
direction1
;
Vec3f
closestOnSegment2
=
center2
+
s2
*
direction2
;
// witness points achieving the distance between the two segments
const
Vec3f
&
w1
=
p1
+
s
*
d1
;
const
Vec3f
&
w2
=
p2
+
t
*
d2
;
FCL_REAL
distance
=
(
w1
-
w2
).
norm
();
Vec3f
normal
=
(
w1
-
w2
)
/
distance
;
Vec3f
unitVector
=
closestOnSegment2
-
closestOnSegment1
;
FCL_REAL
distance
=
sqrt
(
sqrDist
);
if
(
distance
>=
epsilon
)
{
unitVector
/=
distance
;
}
else
{
unitVector
.
setZero
();
// capsule spcecific distance computation
distance
=
distance
-
(
radius1
+
radius2
);
result
.
min_distance
=
distance
;
// witness points for the capsules
if
(
request
.
enable_nearest_points
)
{
result
.
nearest_points
[
0
]
=
w1
-
radius1
*
normal
;
result
.
nearest_points
[
1
]
=
w2
+
radius2
*
normal
;
}
// Update distance result.
result
.
min_distance
=
distance
-
c1
->
radius
-
c2
->
radius
;
if
(
request
.
enable_nearest_points
)
{
result
.
nearest_points
[
0
]
=
closestOnSegment1
+
c1
->
radius
*
unitVector
;
result
.
nearest_points
[
1
]
=
closestOnSegment2
-
c2
->
radius
*
unitVector
;
}
result
.
o1
=
o1
;
result
.
o2
=
o2
;
result
.
b1
=
-
1
;
result
.
b2
=
-
1
;
return
result
.
min_distance
;
return
distance
;
}
template
<
>
...
...
@@ -334,18 +158,22 @@ namespace fcl {
FCL_REAL
distance
=
ShapeShapeDistance
<
Capsule
,
Capsule
>
(
o1
,
tf1
,
o2
,
tf2
,
unused
,
distanceRequest
,
distanceResult
);
if
(
distance
<=
0
)
{
Contact
contact
(
o1
,
o2
,
distanceResult
.
b1
,
distanceResult
.
b2
);
if
(
distance
>
0
)
{
return
0
;
}
else
{
Contact
contact
(
o1
,
o2
,
-
1
,
-
1
);
const
Vec3f
&
p1
=
distanceResult
.
nearest_points
[
0
];
const
Vec3f
&
p2
=
distanceResult
.
nearest_points
[
1
];
contact
.
pos
=
.5
*
(
p1
+
p2
);
contact
.
pos
=
0
.5
*
(
p1
+
p2
);
contact
.
normal
=
(
p2
-
p1
)
/
(
p2
-
p1
).
norm
();
result
.
addContact
(
contact
);
result
.
addContact
(
contact
);
return
1
;
}
result
.
distance_lower_bound
=
distance
;
return
0
;
}
}
// namespace fcl
}
// namespace hpp
test/capsule_capsule.cpp
View file @
6a1930cd
...
...
@@ -308,13 +308,14 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
distance
(
&
o1
,
&
o2
,
distanceRequest
,
distanceResult
);
std
::
cerr
<<
"Applied rotation and translation on two capsules"
<<
std
::
endl
;
std
::
cerr
<<
"R1 = "
<<
tf1
.
getRotation
()
<<
", T1 = "
<<
tf1
.
getTranslation
()
<<
std
::
endl
<<
"R2 = "
<<
tf2
.
getRotation
()
<<
", T2 = "
<<
tf2
.
getTranslation
()
<<
std
::
endl
;
std
::
cerr
<<
"Closest points: p1 = "
<<
distanceResult
.
nearest_points
[
0
]
<<
", p2 = "
<<
distanceResult
.
nearest_points
[
1
]
<<
", distance = "
<<
distanceResult
.
min_distance
<<
std
::
endl
;
std
::
cerr
<<
"R1 = "
<<
tf1
.
getRotation
()
<<
std
::
endl
<<
"T1 = "
<<
tf1
.
getTranslation
().
transpose
()
<<
std
::
endl
<<
"R2 = "
<<
tf2
.
getRotation
()
<<
std
::
endl
<<
"T2 = "
<<
tf2
.
getTranslation
().
transpose
()
<<
std
::
endl
;
std
::
cerr
<<
"Closest points:"
<<
std
::
endl
<<
"p1 = "
<<
distanceResult
.
nearest_points
[
0
].
transpose
()
<<
std
::
endl
<<
"p2 = "
<<
distanceResult
.
nearest_points
[
1
].
transpose
()
<<
std
::
endl
<<
"distance = "
<<
distanceResult
.
min_distance
<<
std
::
endl
;
const
Vec3f
&
p1
=
distanceResult
.
nearest_points
[
0
];
const
Vec3f
&
p2
=
distanceResult
.
nearest_points
[
1
];
...
...
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