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
d5363027
Commit
d5363027
authored
Jun 17, 2014
by
Florent Lamiraux
Browse files
Add a test for distance computation between boxes.
parent
94bddcba
Changes
2
Hide whitespace changes
Inline
Side-by-side
test/CMakeLists.txt
View file @
d5363027
...
...
@@ -32,6 +32,7 @@ add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
add_fcl_test
(
test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp
)
add_fcl_test
(
test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp
)
add_fcl_test
(
test_fcl_box_box_distance test_fcl_box_box_distance.cpp
)
add_fcl_test
(
test_fcl_simple test_fcl_simple.cpp
)
add_fcl_test
(
test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp
)
add_fcl_test
(
test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp
)
...
...
test/test_fcl_box_box_distance.cpp
0 → 100644
View file @
d5363027
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, CNRS.
* 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 Willow Garage, Inc. 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.
*/
/** \author Florent Lamiraux <florent@laas.fr> */
#define BOOST_TEST_MODULE "FCL_BOX_BOX"
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
#include
<boost/test/unit_test.hpp>
#include
<cmath>
#include
<fcl/distance.h>
#include
<fcl/math/transform.h>
#include
<fcl/collision.h>
#include
<fcl/collision_object.h>
#include
<fcl/shape/geometric_shapes.h>
using
namespace
fcl
;
typedef
boost
::
shared_ptr
<
CollisionGeometry
>
CollisionGeometryPtr_t
;
BOOST_AUTO_TEST_CASE
(
distance_box_box_1
)
{
CollisionGeometryPtr_t
s1
(
new
Box
(
6
,
10
,
2
));
CollisionGeometryPtr_t
s2
(
new
Box
(
2
,
2
,
2
));
Transform3f
tf1
;
Transform3f
tf2
(
Vec3f
(
25
,
20
,
5
));
CollisionObject
o1
(
s1
,
tf1
);
CollisionObject
o2
(
s2
,
tf2
);
// Enable computation of nearest points
DistanceRequest
distanceRequest
(
true
);
DistanceResult
distanceResult
;
distance
(
&
o1
,
&
o2
,
distanceRequest
,
distanceResult
);
std
::
cerr
<<
"Applied translation on two boxes"
;
std
::
cerr
<<
" T1 = "
<<
tf1
.
getTranslation
()
<<
", 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
;
double
dx
=
25
-
3
-
1
;
double
dy
=
20
-
5
-
1
;
double
dz
=
5
-
1
-
1
;
const
Vec3f
&
p1
=
distanceResult
.
nearest_points
[
0
];
const
Vec3f
&
p2
=
distanceResult
.
nearest_points
[
1
];
BOOST_CHECK_CLOSE
(
distanceResult
.
min_distance
,
sqrt
(
dx
*
dx
+
dy
*
dy
+
dz
*
dz
),
1e-4
);
BOOST_CHECK_CLOSE
(
p1
[
0
],
3
,
1e-6
);
BOOST_CHECK_CLOSE
(
p1
[
1
],
5
,
1e-6
);
BOOST_CHECK_CLOSE
(
p1
[
2
],
1
,
1e-6
);
BOOST_CHECK_CLOSE
(
p2
[
0
],
24
,
1e-6
);
BOOST_CHECK_CLOSE
(
p2
[
1
],
19
,
1e-6
);
BOOST_CHECK_CLOSE
(
p2
[
2
],
4
,
1e-6
);
}
#if 0
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformXY)
{
CollisionGeometryPtr_t s1 (new Capsule (5, 10));
CollisionGeometryPtr_t s2 (new Capsule (5, 10));
Transform3f tf1;
Transform3f tf2 (Vec3f(20, 20,0));
CollisionObject o1 (s1, tf1);
CollisionObject o2 (s2, tf2);
// Enable computation of nearest points
DistanceRequest distanceRequest (true);
DistanceResult distanceResult;
distance (&o1, &o2, distanceRequest, distanceResult);
std::cerr << "Applied translation on two capsules";
std::cerr << " T1 = " << tf1.getTranslation()
<< ", 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;
FCL_REAL expected = sqrt(800) - 10;
BOOST_CHECK_CLOSE(distanceResult.min_distance, expected, 1e-6);
}
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ)
{
CollisionGeometryPtr_t s1 (new Capsule (5, 10));
CollisionGeometryPtr_t s2 (new Capsule (5, 10));
Transform3f tf1;
Transform3f tf2 (Vec3f(0,0,20.1));
CollisionObject o1 (s1, tf1);
CollisionObject o2 (s2, tf2);
// Enable computation of nearest points
DistanceRequest distanceRequest (true);
DistanceResult distanceResult;
distance (&o1, &o2, distanceRequest, distanceResult);
std::cerr << "Applied translation on two capsules";
std::cerr << " T1 = " << tf1.getTranslation()
<< ", 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;
BOOST_CHECK_CLOSE(distanceResult.min_distance, 0.1, 1e-6);
}
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
{
CollisionGeometryPtr_t s1 (new Capsule (5, 10));
CollisionGeometryPtr_t s2 (new Capsule (5, 10));
Transform3f tf1;
Transform3f tf2 (Quaternion3f (sqrt (2)/2, 0, sqrt (2)/2, 0),
Vec3f(0,0,25.1));
CollisionObject o1 (s1, tf1);
CollisionObject o2 (s2, tf2);
// Enable computation of nearest points
DistanceRequest distanceRequest (true);
DistanceResult distanceResult;
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;
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
BOOST_CHECK_CLOSE(distanceResult.min_distance, 10.1, 1e-6);
CHECK_CLOSE_TO_0 (p1 [0], 1e-4);
CHECK_CLOSE_TO_0 (p1 [1], 1e-4);
BOOST_CHECK_CLOSE (p1 [2], 10, 1e-4);
CHECK_CLOSE_TO_0 (p2 [0], 1e-4);
CHECK_CLOSE_TO_0 (p2 [1], 1e-4);
BOOST_CHECK_CLOSE (p2 [2], 20.1, 1e-4);
}
#endif
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