Commit 63ed929f authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Split test_fcl_capsule_box into two tests:

  one is working, the other one fails to compute the nearest points although
  it properly computes the distance.
parent 2f57a4e4
......@@ -30,7 +30,8 @@ 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_simple test_fcl_simple.cpp)
add_fcl_test(test_fcl_capsule_box test_fcl_capsule_box.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)
#add_fcl_test(test_fcl_global_penetration test_fcl_global_penetration.cpp libsvm/svm.cpp test_fcl_utility.cpp)
if (FCL_HAVE_OCTOMAP)
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, CNRS-LAAS
* 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 CNRS-LAAS 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 */
#define BOOST_TEST_MODULE "FCL_GEOMETRIC_SHAPES"
#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>
BOOST_AUTO_TEST_CASE(distance_capsule_box)
{
typedef boost::shared_ptr <fcl::CollisionGeometry> CollisionGeometryPtr_t;
// Capsule of radius 2 and of height 4
CollisionGeometryPtr_t capsuleGeometry (new fcl::Capsule (2., 4.));
// Box of size 1 by 2 by 4
CollisionGeometryPtr_t boxGeometry (new fcl::Box (1., 2., 4.));
// Enable computation of nearest points
fcl::DistanceRequest distanceRequest (true);
fcl::DistanceResult distanceResult;
fcl::Transform3f tf1 (fcl::Vec3f (3., 0, 0));
fcl::Transform3f tf2;
fcl::CollisionObject capsule (capsuleGeometry, tf1);
fcl::CollisionObject box (boxGeometry, tf2);
// test distance
fcl::distance (&capsule, &box, distanceRequest, distanceResult);
// Nearest point on capsule
fcl::Vec3f o1 (distanceResult.nearest_points [0]);
// Nearest point on box
fcl::Vec3f o2 (distanceResult.nearest_points [1]);
BOOST_CHECK_CLOSE (distanceResult.min_distance, 0.5, 1e-4);
BOOST_CHECK_CLOSE (o1 [0], -2.0, 1e-4);
CHECK_CLOSE_TO_0 (o1 [1], 1e-4);
BOOST_CHECK_CLOSE (o2 [0], 0.5, 1e-4);
BOOST_CHECK_CLOSE (o1 [1], 0.0, 1e-4);
// Move capsule above box
tf1 = fcl::Transform3f (fcl::Vec3f (0., 0., 8.));
capsule.setTransform (tf1);
// test distance
distanceResult.clear ();
fcl::distance (&capsule, &box, distanceRequest, distanceResult);
o1 = distanceResult.nearest_points [0];
o2 = distanceResult.nearest_points [1];
BOOST_CHECK_CLOSE (distanceResult.min_distance, 2.0, 1e-4);
CHECK_CLOSE_TO_0 (o1 [0], 1e-4);
CHECK_CLOSE_TO_0 (o1 [1], 1e-4);
BOOST_CHECK_CLOSE (o1 [2], -4.0, 1e-4);
CHECK_CLOSE_TO_0 (o2 [0], 1e-4);
CHECK_CLOSE_TO_0 (o2 [1], 1e-4);
BOOST_CHECK_CLOSE (o2 [2], 2.0, 1e-4);
// Rotate capsule around y axis by pi/2 and move it behind box
tf1.setTranslation (fcl::Vec3f (-10., 0., 0.));
tf1.setQuatRotation (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0));
capsule.setTransform (tf1);
// test distance
distanceResult.clear ();
fcl::distance (&capsule, &box, distanceRequest, distanceResult);
o1 = distanceResult.nearest_points [0];
o2 = distanceResult.nearest_points [1];
BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-4);
CHECK_CLOSE_TO_0 (o1 [0], 1e-4);
CHECK_CLOSE_TO_0 (o1 [1], 1e-4);
BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-4);
BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-4);
CHECK_CLOSE_TO_0 (o2 [1], 1e-4);
CHECK_CLOSE_TO_0 (o2 [2], 1e-4);
}
......@@ -36,8 +36,10 @@
#define BOOST_TEST_MODULE "FCL_GEOMETRIC_SHAPES"
#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>
......@@ -55,20 +57,25 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
// Enable computation of nearest points
fcl::DistanceRequest distanceRequest (true);
fcl::DistanceResult distanceResult;
fcl::Transform3f tf1 (fcl::Vec3f (3., 0, 0));
// Rotate capsule around y axis by pi/2 and move it behind box
fcl::Transform3f tf1 (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0),
fcl::Vec3f (-10., 0.8, 1.5));
fcl::Transform3f tf2;
fcl::CollisionObject capsule (capsuleGeometry, tf1);
fcl::CollisionObject box (boxGeometry, tf2);
// test distance
distanceResult.clear ();
fcl::distance (&capsule, &box, distanceRequest, distanceResult);
// Nearest point on capsule
const fcl::Vec3f& o1 (distanceResult.nearest_points [0]);
// Nearest point on box
const fcl::Vec3f& o2 (distanceResult.nearest_points [1]);
BOOST_CHECK_CLOSE (distanceResult.min_distance, 0.5, 1e-4);
BOOST_CHECK_CLOSE (o1 [0], -2, 1e-4);
BOOST_CHECK_CLOSE (o1 [1], 0, 1e-4);
BOOST_CHECK_CLOSE (o2 [0], .5, 1e-4);
BOOST_CHECK_CLOSE (o1 [1], 0, 1e-4);
fcl::Vec3f o1 = distanceResult.nearest_points [0];
fcl::Vec3f o2 = distanceResult.nearest_points [1];
BOOST_CHECK_CLOSE (distanceResult.min_distance, 5.5, 1e-4);
CHECK_CLOSE_TO_0 (o1 [0], 1e-4);
CHECK_CLOSE_TO_0 (o1 [1], 1e-4);
BOOST_CHECK_CLOSE (o1 [2], 4.0, 1e-4);
BOOST_CHECK_CLOSE (o2 [0], -0.5, 1e-4);
BOOST_CHECK_CLOSE (o2 [1], 0.8, 1e-4);
BOOST_CHECK_CLOSE (o2 [2], 1.5, 1e-4);
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment