Commit c1ec91a7 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add a test on distance computation between capsule and box.

  - test fails on current version.
parent f8da2437
......@@ -30,6 +30,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_simple test_fcl_simple.cpp)
add_fcl_test(test_fcl_capsule_box test_fcl_capsule_box.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"
#include <boost/test/unit_test.hpp>
#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);
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);
}
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