diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 66005fcb477c61ade405d7313fd213cd5d64d11b..b76c5c2d21b31b2e7f53fa34090cf6212f8bf7e7 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -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)
diff --git a/test/test_fcl_distance_lower_bound.cpp b/test/test_fcl_distance_lower_bound.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3f6047484067b36add2133c029feabec7255ab11
--- /dev/null
+++ b/test/test_fcl_distance_lower_bound.cpp
@@ -0,0 +1,203 @@
+/*
+ * 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);
+ }
+ }
+}