Commit 5261d92b authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add a test on distance lower bound.

parent 6e699b0c
......@@ -15,6 +15,8 @@ include_directories(${Boost_INCLUDE_DIRS})
add_fcl_test(test_fcl_collision test_fcl_collision.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_distance test_fcl_distance.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_distance_lower_bound test_fcl_distance_lower_bound.cpp
test_fcl_utility.cpp)
add_fcl_test(test_fcl_geometric_shapes test_fcl_geometric_shapes.cpp test_fcl_utility.cpp)
#add_fcl_test(test_fcl_broadphase test_fcl_broadphase.cpp test_fcl_utility.cpp)
#add_fcl_test(test_fcl_shape_mesh_consistency test_fcl_shape_mesh_consistency.cpp test_fcl_utility.cpp)
......
/*
* 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.
*/
#define BOOST_TEST_MODULE "FCL_DISTANCE_LOWER_BOUND"
# include <boost/test/included/unit_test.hpp>
# include <boost/filesystem.hpp>
# include <fcl/fwd.hh>
# include <fcl/data_types.h>
# include <fcl/BV/OBBRSS.h>
# include <fcl/BVH/BVH_model.h>
# include <fcl/narrowphase/narrowphase.h>
# include <fcl/collision.h>
# include <fcl/distance.h>
# include "test_fcl_utility.h"
# include "fcl_resources/config.h"
using fcl::Transform3f;
using fcl::Vec3f;
using fcl::Triangle;
using fcl::OBBRSS;
using fcl::BVHModel;
using fcl::CollisionResult;
using fcl::CollisionRequest;
using fcl::DistanceRequest;
using fcl::DistanceResult;
using fcl::CollisionObject;
using fcl::CollisionGeometryPtr_t;
using fcl::FCL_REAL;
bool testDistanceLowerBound (const Transform3f& tf,
const CollisionGeometryPtr_t& m1,
const CollisionGeometryPtr_t& m2,
FCL_REAL& distance)
{
Transform3f pose1(tf), pose2;
CollisionRequest request;
request.enable_distance_lower_bound = true;
CollisionResult result;
CollisionObject co1 (m1, pose1);
CollisionObject co2 (m2, pose2);
fcl::collide(&co1, &co2, request, result);
distance = result.distance_lower_bound;
return result.isCollision ();
}
bool testCollide (const Transform3f& tf, const CollisionGeometryPtr_t& m1,
const CollisionGeometryPtr_t& m2)
{
Transform3f pose1(tf), pose2;
CollisionRequest request;
request.enable_distance_lower_bound = false;
CollisionResult result;
CollisionObject co1 (m1, pose1);
CollisionObject co2 (m2, pose2);
fcl::collide(&co1, &co2, request, result);
return result.isCollision ();
}
bool testDistance (const Transform3f& tf, const CollisionGeometryPtr_t& m1,
const CollisionGeometryPtr_t& m2, FCL_REAL& distance)
{
Transform3f pose1(tf), pose2;
DistanceRequest request;
DistanceResult result;
CollisionObject co1 (m1, pose1);
CollisionObject co2 (m2, pose2);
fcl::distance (&co1, &co2, request, result);
distance = result.min_distance;
if(result.min_distance <= 0) {
return true;
}
else {
return false;
}
}
BOOST_AUTO_TEST_CASE(mesh_mesh)
{
std::vector<Vec3f> p1, p2;
std::vector<Triangle> t1, t2;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
loadOBJFile((path / "rob.obj").string().c_str(), p2, t2);
boost::shared_ptr < BVHModel <OBBRSS> > m1 (new BVHModel <OBBRSS>);
boost::shared_ptr < BVHModel <OBBRSS> > m2 (new BVHModel <OBBRSS>);
m1->beginModel();
m1->addSubModel(p1, t1);
m1->endModel();
m2->beginModel();
m2->addSubModel(p2, t2);
m2->endModel();
std::vector<Transform3f> transforms;
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
std::size_t n = 100;
bool verbose = false;
generateRandomTransforms(extents, transforms, n);
// collision
for(std::size_t i = 0; i < transforms.size(); ++i)
{
FCL_REAL distanceLowerBound, distance;
bool col1, col2, col3;
col1 = testDistanceLowerBound (transforms[i], m1, m2, distanceLowerBound);
col2 = testDistance (transforms[i], m1, m2, distance);
col3 = testCollide (transforms[i], m1, m2);
std::cout << "col1 = " << col1 << ", col2 = " << col2
<< ", col3 = " << col3 << std::endl;
std::cout << "distance lower bound = " << distanceLowerBound << std::endl;
std::cout << "distance = " << distance << std::endl;
BOOST_CHECK (col1 == col3);
if (!col1) {
BOOST_CHECK_MESSAGE (distanceLowerBound <= distance,
"distance = " << distance << ", lower bound = "
<< distanceLowerBound);
}
}
}
BOOST_AUTO_TEST_CASE(box_mesh)
{
std::vector<Vec3f> p1;
std::vector<Triangle> t1;
boost::filesystem::path path(TEST_RESOURCES_DIR);
loadOBJFile((path / "env.obj").string().c_str(), p1, t1);
boost::shared_ptr < BVHModel <OBBRSS> > m1 (new BVHModel <OBBRSS>);
boost::shared_ptr < fcl::Box > m2 (new fcl::Box (500, 200, 150));
m1->beginModel();
m1->addSubModel(p1, t1);
m1->endModel();
std::vector<Transform3f> transforms;
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
std::size_t n = 100;
bool verbose = false;
generateRandomTransforms(extents, transforms, n);
// collision
for(std::size_t i = 0; i < transforms.size(); ++i)
{
FCL_REAL distanceLowerBound, distance;
bool col1, col2;
col1 = testDistanceLowerBound (transforms[i], m1, m2, distanceLowerBound);
col2 = testDistance (transforms[i], m1, m2, distance);
BOOST_CHECK (col1 == col2);
if (!col1) {
BOOST_CHECK_MESSAGE (distanceLowerBound <= distance,
"distance = " << distance << ", lower bound = "
<< distanceLowerBound);
}
}
}
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