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
1815f3db
Unverified
Commit
1815f3db
authored
Dec 16, 2019
by
Joseph Mirabel
Committed by
GitHub
Dec 16, 2019
Browse files
Merge pull request #119 from rstrudel/devel
Capsule/Capsule distance function: reimplementation
parents
a5223c91
9e4d930b
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/distance_capsule_capsule.cpp
View file @
1815f3db
// Geometric Tools, LLC
// Copyright (c) 1998-2011
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
//
// Modified by Florent Lamiraux 2014
/*
* Software License Agreement (BSD License)
* Copyright (c) 2015-2019, CNRS - LAAS INRIA
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include
<cmath>
#include
<limits>
...
...
@@ -12,6 +37,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 +52,110 @@ 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
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
(
a
<=
EPSILON
&&
e
<=
EPSILON
)
{
// Check if the segments degenerate into points
s
=
t
=
0.0
;
}
else
if
(
a
<=
EPSILON
)
{
// First segment is degenerated
s
=
0.0
;
t
=
CLAMP
(
f
/
e
,
0.0
,
1.0
);
}
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 colinear
FCL_REAL
denom
=
fmax
(
a
*
e
-
b
*
b
,
0
);
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
;
}
}
}
}
if
(
denom
>
EPSILON
)
{
s
=
CLAMP
((
b
*
f
-
c
*
e
)
/
denom
,
0.0
,
1.0
);
}
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
;
}
}
}
else
{
s
=
0.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
;
t
=
(
b
*
s
+
f
)
/
e
;
if
(
t
<
0.0
)
{
t
=
0.0
;
s
=
CLAMP
(
-
c
/
a
,
0.0
,
1.0
);
}
else
if
(
lambda
>
e0pe1
)
{
lambda
=
e0pe1
;
else
if
(
t
>
1.0
)
{
t
=
1.0
;
s
=
CLAMP
((
b
-
c
)
/
a
,
0.0
,
1.0
);
}
s2
=
-
sign
*
lambda
*
halfLength2
/
e0pe1
;
s1
=
lambda
+
sign
*
s2
;
sqrDist
=
lambda
*
(
lambda
+
2.0
*
b0Avr
)
+
c
;
}
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
;
result
.
normal
=
normal
;
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 +172,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
.
normal
=
(
p2
-
p1
)
/
(
p2
-
p1
)
.
norm
()
;
result
.
addContact
(
contact
);
contact
.
pos
=
0
.5
*
(
p1
+
p2
);
contact
.
normal
=
distanceResult
.
norm
al
;
result
.
addContact
(
contact
);
return
1
;
}
result
.
distance_lower_bound
=
distance
;
return
0
;
}
}
// namespace fcl
}
// namespace hpp
test/capsule_capsule.cpp
View file @
1815f3db
...
...
@@ -45,6 +45,7 @@
#include
<cmath>
#include
<iostream>
#include
<hpp/fcl/distance.h>
#include
<hpp/fcl/collision.h>
#include
<hpp/fcl/math/transform.h>
#include
<hpp/fcl/collision.h>
#include
<hpp/fcl/collision_object.h>
...
...
@@ -55,6 +56,163 @@
using
namespace
hpp
::
fcl
;
typedef
boost
::
shared_ptr
<
CollisionGeometry
>
CollisionGeometryPtr_t
;
BOOST_AUTO_TEST_CASE
(
collision_capsule_capsule_trivial
)
{
const
double
radius
=
1.
;
CollisionGeometryPtr_t
c1
(
new
Capsule
(
radius
,
0.
));
CollisionGeometryPtr_t
c2
(
new
Capsule
(
radius
,
0.
));
CollisionGeometryPtr_t
s1
(
new
Sphere
(
radius
));
CollisionGeometryPtr_t
s2
(
new
Sphere
(
radius
));
#ifndef NDEBUG
int
num_tests
=
1e3
;
#else
int
num_tests
=
1e6
;
#endif
Transform3f
tf1
;
Transform3f
tf2
;
for
(
int
i
=
0
;
i
<
num_tests
;
++
i
)
{
Eigen
::
Vector3d
p1
=
Eigen
::
Vector3d
::
Random
()
*
(
2.
*
radius
);
Eigen
::
Vector3d
p2
=
Eigen
::
Vector3d
::
Random
()
*
(
2.
*
radius
);
Eigen
::
Matrix3d
rot1
=
Eigen
::
Quaterniond
(
Eigen
::
Vector4d
::
Random
().
normalized
()).
toRotationMatrix
();
Eigen
::
Matrix3d
rot2
=
Eigen
::
Quaterniond
(
Eigen
::
Vector4d
::
Random
().
normalized
()).
toRotationMatrix
();
tf1
.
setTranslation
(
p1
);
tf1
.
setRotation
(
rot1
);
tf2
.
setTranslation
(
p2
);
tf2
.
setRotation
(
rot2
);
CollisionObject
capsule_o1
(
c1
,
tf1
);
CollisionObject
capsule_o2
(
c2
,
tf2
);
CollisionObject
sphere_o1
(
s1
,
tf1
);
CollisionObject
sphere_o2
(
s2
,
tf2
);
// Enable computation of nearest points
CollisionRequest
collisionRequest
;
CollisionResult
capsule_collisionResult
,
sphere_collisionResult
;
size_t
sphere_num_collisions
=
collide
(
&
sphere_o1
,
&
sphere_o2
,
collisionRequest
,
sphere_collisionResult
);
size_t
capsule_num_collisions
=
collide
(
&
capsule_o1
,
&
capsule_o2
,
collisionRequest
,
capsule_collisionResult
);
BOOST_CHECK
(
sphere_num_collisions
==
capsule_num_collisions
);
}
}
BOOST_AUTO_TEST_CASE
(
collision_capsule_capsule_aligned
)
{
const
double
radius
=
0.01
;
const
double
length
=
0.2
;
CollisionGeometryPtr_t
c1
(
new
Capsule
(
radius
,
length
));
CollisionGeometryPtr_t
c2
(
new
Capsule
(
radius
,
length
));
#ifndef NDEBUG
int
num_tests
=
1e3
;
#else
int
num_tests
=
1e6
;
#endif
Transform3f
tf1
;
Transform3f
tf2
;
Eigen
::
Vector3d
p1
=
Eigen
::
Vector3d
::
Zero
();
Eigen
::
Vector3d
p2_no_collision
=
Eigen
::
Vector3d
(
0.
,
0.
,
2
*
(
length
/
2.
+
radius
)
+
1e-3
);
// because capsule are along the Z axis
for
(
int
i
=
0
;
i
<
num_tests
;
++
i
)
{
Eigen
::
Matrix3d
rot
=
Eigen
::
Quaterniond
(
Eigen
::
Vector4d
::
Random
().
normalized
()).
toRotationMatrix
();
tf1
.
setTranslation
(
p1
);
tf1
.
setRotation
(
rot
);
tf2
.
setTranslation
(
p2_no_collision
);
tf2
.
setRotation
(
rot
);
CollisionObject
capsule_o1
(
c1
,
tf1
);
CollisionObject
capsule_o2
(
c2
,
tf2
);
// Enable computation of nearest points
CollisionRequest
collisionRequest
;
CollisionResult
capsule_collisionResult
;
size_t
capsule_num_collisions
=
collide
(
&
capsule_o1
,
&
capsule_o2
,
collisionRequest
,
capsule_collisionResult
);
BOOST_CHECK
(
capsule_num_collisions
==
0
);
}
Eigen
::
Vector3d
p2_with_collision
=
Eigen
::
Vector3d
(
0.
,
0.
,
std
::
min
(
length
/
2.
,
radius
)
*
(
1.
-
1e-2
));
for
(
int
i
=
0
;
i
<
num_tests
;
++
i
)
{
Eigen
::
Matrix3d
rot
=
Eigen
::
Quaterniond
(
Eigen
::
Vector4d
::
Random
().
normalized
()).
toRotationMatrix
();
tf1
.
setTranslation
(
p1
);
tf1
.
setRotation
(
rot
);
tf2
.
setTranslation
(
p2_with_collision
);
tf2
.
setRotation
(
rot
);
CollisionObject
capsule_o1
(
c1
,
tf1
);
CollisionObject
capsule_o2
(
c2
,
tf2
);
// Enable computation of nearest points
CollisionRequest
collisionRequest
;
CollisionResult
capsule_collisionResult
;
size_t
capsule_num_collisions
=
collide
(
&
capsule_o1
,
&
capsule_o2
,
collisionRequest
,
capsule_collisionResult
);
BOOST_CHECK
(
capsule_num_collisions
>
0
);
}
p2_no_collision
=
Eigen
::
Vector3d
(
0.
,
0.
,
2
*
(
length
/
2.
+
radius
)
+
1e-3
);
Transform3f
geom1_placement
(
Eigen
::
Matrix3d
::
Identity
(),
Eigen
::
Vector3d
::
Zero
());
Transform3f
geom2_placement
(
Eigen
::
Matrix3d
::
Identity
(),
p2_no_collision
);
for
(
int
i
=
0
;
i
<
num_tests
;
++
i
)
{
Eigen
::
Matrix3d
rot
=
Eigen
::
Quaterniond
(
Eigen
::
Vector4d
::
Random
().
normalized
()).
toRotationMatrix
();
Eigen
::
Vector3d
trans
=
Eigen
::
Vector3d
::
Random
();
Transform3f
displacement
(
rot
,
trans
);
Transform3f
tf1
=
displacement
*
geom1_placement
;
Transform3f
tf2
=
displacement
*
geom2_placement
;
CollisionObject
capsule_o1
(
c1
,
tf1
);
CollisionObject
capsule_o2
(
c2
,
tf2
);