Commit 77c60417 authored by isucan's avatar isucan
Browse files

fix lib location in manifest, switch from gtest to boost test, add resource locations

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@185 253336fb-580f-4252-a368-f3cef5a2a82b
parent 0d7ffb77
......@@ -44,13 +44,14 @@ else()
message(STATUS "FCL does not use Octomap")
endif()
find_package(Boost COMPONENTS thread date_time filesystem system unit_test_framework REQUIRED)
include_directories(${Boost_INCLUDE_DIR})
if(MSVC OR MSVC90 OR MSVC10)
add_definitions(-DBOOST_ALL_NO_LIB)
endif()
find_package(Boost COMPONENTS thread system REQUIRED)
include_directories(${Boost_INCLUDE_DIR})
add_definitions(-DBOOST_TEST_DYN_LINK)
# make sure we know what flag we used for SSE
include_directories("include")
if(PKG_CONFIG_FOUND)
......@@ -88,39 +89,4 @@ install(FILES "${pkg_conf_file}" DESTINATION lib/pkgconfig/ COMPONENT pkgconfig)
enable_testing()
find_package(GTest REQUIRED)
include_directories(${GTEST_INCLUDE_DIRS})
add_executable(test_fcl_collision test/test_fcl_collision.cpp test/test_fcl_utility.cpp)
target_link_libraries(test_fcl_collision ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
GTEST_ADD_TESTS(test_fcl_collision "" test/test_fcl_collision.cpp)
add_executable(test_fcl_distance test/test_fcl_distance.cpp test/test_fcl_utility.cpp)
target_link_libraries(test_fcl_distance ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
GTEST_ADD_TESTS(test_fcl_distance "" test/test_fcl_distance.cpp)
add_executable(test_fcl_geometric_shapes test/test_fcl_geometric_shapes.cpp test/test_fcl_utility.cpp)
target_link_libraries(test_fcl_geometric_shapes ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
GTEST_ADD_TESTS(test_fcl_geometric_shapes "" test/test_fcl_geometric_shapes.cpp)
add_executable(test_fcl_broadphase test/test_fcl_broadphase.cpp test/test_fcl_utility.cpp)
target_link_libraries(test_fcl_broadphase ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
GTEST_ADD_TESTS(test_fcl_broadphase "" test/test_fcl_broadphase.cpp)
add_executable(test_fcl_shape_mesh_consistency test/test_fcl_shape_mesh_consistency.cpp test/test_fcl_utility.cpp)
target_link_libraries(test_fcl_shape_mesh_consistency ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
GTEST_ADD_TESTS(test_fcl_shape_mesh_consistency "" test/test_fcl_shape_mesh_consistency.cpp)
add_executable(test_fcl_frontlist test/test_fcl_frontlist.cpp test/test_fcl_utility.cpp)
target_link_libraries(test_fcl_frontlist ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
GTEST_ADD_TESTS(test_fcl_frontlist "" test/test_fcl_frontlist.cpp)
add_executable(test_fcl_math test/test_fcl_math.cpp test/test_fcl_utility.cpp)
target_link_libraries(test_fcl_math ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
GTEST_ADD_TESTS(test_fcl_math "" test/test_fcl_math.cpp)
if (FCL_HAVE_OCTOMAP)
add_executable(test_fcl_octomap test/test_fcl_octomap.cpp test/test_fcl_utility.cpp)
target_link_libraries(test_fcl_octomap ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES})
GTEST_ADD_TESTS(test_fcl_octomap "" test/test_fcl_octomap.cpp)
endif()
add_subdirectory(test)
......@@ -14,7 +14,7 @@
<rosdep name="octomap" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lfcl"/>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/build/lib -L${prefix}/build/lib -lfcl"/>
</export>
</package>
macro(add_fcl_test test_name)
add_executable(${ARGV})
target_link_libraries(${test_name}
fcl
${Boost_SYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY}
${Boost_DATE_TIME_LIBRARY}
${Boost_UNIT_TEST_FRAMEWORK_LIBRARY})
add_test(${test_name} ${EXECUTABLE_OUTPUT_PATH}/${test_name})
endmacro(add_fcl_test)
# configure location of resources
file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/fcl_resources" TEST_RESOURCES_DIR)
if(WIN32)
# Correct directory separator for Windows
string(REPLACE "\\" "\\\\" TEST_RESOURCES_DIR ${TEST_RESOURCES_DIR})
endif(WIN32)
configure_file("${TEST_RESOURCES_DIR}/config.h.in" "${TEST_RESOURCES_DIR}/config.h")
include_directories(.)
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_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)
add_fcl_test(test_fcl_frontlist test_fcl_frontlist.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
if (FCL_HAVE_OCTOMAP)
add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
endif()
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Rice University.
* 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 the Rice University 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: Mark Moll */
#ifndef FCL_TEST_RESOURCES_CONFIG_
#define FCL_TEST_RESOURCES_CONFIG_
#define TEST_RESOURCES_DIR "@TEST_RESOURCES_DIR@"
#endif
......@@ -34,11 +34,14 @@
/** \author Jia Pan */
#define BOOST_TEST_MODULE "FCL_BROADPHASE"
#include <boost/test/unit_test.hpp>
#include "fcl/broadphase/broadphase.h"
#include "fcl/shape/geometric_shape_to_BVH_model.h"
#include "fcl/math/transform.h"
#include "test_fcl_utility.h"
#include <gtest/gtest.h>
#if USE_GOOGLEHASH
#include <sparsehash/sparse_hash_map>
......@@ -47,6 +50,8 @@
#endif
#include <boost/math/constants/constants.hpp>
#include <iostream>
#include <iomanip>
using namespace fcl;
......@@ -106,28 +111,28 @@ struct GoogleDenseHashTable : public google::dense_hash_map<U, V, std::tr1::hash
#endif
/// check the update, only return collision or not
TEST(test_core_bf, broad_phase_update_collision_binary)
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_binary)
{
broad_phase_update_collision_test(2000, 100, 1000, 1, false);
broad_phase_update_collision_test(2000, 1000, 1000, 1, false);
}
/// check the update, return 10 contacts
TEST(test_core_bf, broad_phase_update_collision)
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision)
{
broad_phase_update_collision_test(2000, 100, 1000, 10, false);
broad_phase_update_collision_test(2000, 1000, 1000, 10, false);
}
/// check the update, exhaustive
TEST(test_core_bf, broad_phase_update_collision_exhaustive)
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_update_collision_exhaustive)
{
broad_phase_update_collision_test(2000, 100, 1000, 1, true);
broad_phase_update_collision_test(2000, 1000, 1000, 1, true);
}
/// check broad phase distance
TEST(test_core_bf, broad_phase_distance)
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_distance)
{
broad_phase_distance_test(200, 100, 100);
broad_phase_distance_test(200, 1000, 100);
......@@ -136,7 +141,7 @@ TEST(test_core_bf, broad_phase_distance)
}
/// check broad phase self distance
TEST(test_core_bf, broad_phase_self_distance)
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance)
{
broad_phase_self_distance_test(200, 512);
broad_phase_self_distance_test(200, 1000);
......@@ -144,7 +149,7 @@ TEST(test_core_bf, broad_phase_self_distance)
}
/// check broad phase collision and self collision, only return collision or not
TEST(test_core_bf, broad_phase_collision_binary)
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary)
{
broad_phase_collision_test(2000, 100, 1000, 1, false);
broad_phase_collision_test(2000, 1000, 1000, 1, false);
......@@ -153,35 +158,35 @@ TEST(test_core_bf, broad_phase_collision_binary)
}
/// check broad phase collision and self collision, return 10 contacts
TEST(test_core_bf, broad_phase_collision)
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision)
{
broad_phase_collision_test(2000, 100, 1000, 10, false);
broad_phase_collision_test(2000, 1000, 1000, 10, false);
}
/// check broad phase update, in mesh, only return collision or not
TEST(test_core_mesh_bf, broad_phase_update_collision_mesh_binary)
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh_binary)
{
broad_phase_update_collision_test(2000, 100, 1000, false, true);
broad_phase_update_collision_test(2000, 1000, 1000, false, true);
}
/// check broad phase update, in mesh, return 10 contacts
TEST(test_core_mesh_bf, broad_phase_update_collision_mesh)
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh)
{
broad_phase_update_collision_test(2000, 100, 1000, 10, false, true);
broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true);
}
/// check broad phase update, in mesh, exhaustive
TEST(test_core_mesh_bf, broad_phase_update_collision_mesh_exhaustive)
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive)
{
broad_phase_update_collision_test(2000, 100, 1000, 1, true, true);
broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true);
}
/// check broad phase distance
TEST(test_core_mesh_bf, broad_phase_distance_mesh)
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_distance_mesh)
{
broad_phase_distance_test(200, 100, 100, true);
broad_phase_distance_test(200, 1000, 100, true);
......@@ -190,7 +195,7 @@ TEST(test_core_mesh_bf, broad_phase_distance_mesh)
}
/// check broad phase self distance
TEST(test_core_mesh_bf, broad_phase_self_distance_mesh)
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_self_distance_mesh)
{
broad_phase_self_distance_test(200, 512, true);
broad_phase_self_distance_test(200, 1000, true);
......@@ -198,35 +203,26 @@ TEST(test_core_mesh_bf, broad_phase_self_distance_mesh)
}
/// check broad phase collision and self collision, return only collision or not, in mesh
TEST(test_core_mesh_bf, broad_phase_collision_mesh_binary)
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary)
{
broad_phase_collision_test(2000, 100, 1000, 1, false, true);
broad_phase_collision_test(2000, 1000, 1000, 1, false, true);
}
/// check broad phase collision and self collision, return 10 contacts, in mesh
TEST(test_core_mesh_bf, broad_phase_collision_mesh)
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh)
{
broad_phase_collision_test(2000, 100, 1000, 10, false, true);
broad_phase_collision_test(2000, 1000, 1000, 10, false, true);
}
/// check broad phase collision and self collision, exhaustive, in mesh
TEST(test_core_mesh_bf, broad_phase_collision_mesh_exhaustive)
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive)
{
broad_phase_collision_test(2000, 100, 1000, 1, true, true);
broad_phase_collision_test(2000, 1000, 1000, 1, true, true);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
void generateEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
{
FCL_REAL extents[] = {-env_scale, env_scale, -env_scale, env_scale, -env_scale, env_scale};
......@@ -512,7 +508,7 @@ void broad_phase_collision_test(double env_scale, std::size_t env_size, std::siz
if(exhaustive)
{
for(size_t i = 1; i < managers.size(); ++i)
ASSERT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts());
}
else
{
......@@ -521,10 +517,10 @@ void broad_phase_collision_test(double env_scale, std::size_t env_size, std::siz
self_res[i] = (self_data[i].result.numContacts() > 0);
for(size_t i = 1; i < self_res.size(); ++i)
ASSERT_TRUE(self_res[0] == self_res[i]);
BOOST_CHECK(self_res[0] == self_res[i]);
for(size_t i = 1; i < managers.size(); ++i)
ASSERT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts());
}
......@@ -553,7 +549,7 @@ void broad_phase_collision_test(double env_scale, std::size_t env_size, std::siz
if(exhaustive)
{
for(size_t j = 1; j < managers.size(); ++j)
ASSERT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts());
}
else
{
......@@ -561,10 +557,10 @@ void broad_phase_collision_test(double env_scale, std::size_t env_size, std::siz
for(size_t j = 0; j < query_res.size(); ++j)
query_res[j] = (query_data[j].result.numContacts() > 0);
for(size_t j = 1; j < query_res.size(); ++j)
ASSERT_TRUE(query_res[0] == query_res[j]);
BOOST_CHECK(query_res[0] == query_res[j]);
for(size_t j = 1; j < managers.size(); ++j)
ASSERT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts());
}
}
......@@ -690,7 +686,7 @@ void broad_phase_self_distance_test(double env_scale, std::size_t env_size, bool
// std::cout << std::endl;
for(size_t i = 1; i < managers.size(); ++i)
ASSERT_TRUE(fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) < DELTA ||
BOOST_CHECK(fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) < DELTA ||
fabs(self_data[0].result.min_distance - self_data[i].result.min_distance) / fabs(self_data[0].result.min_distance) < DELTA);
for(size_t i = 0; i < env.size(); ++i)
......@@ -834,7 +830,7 @@ void broad_phase_distance_test(double env_scale, std::size_t env_size, std::size
// std::cout << std::endl;
for(size_t j = 1; j < managers.size(); ++j)
ASSERT_TRUE(fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) < DELTA ||
BOOST_CHECK(fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) < DELTA ||
fabs(query_data[0].result.min_distance - query_data[j].result.min_distance) / fabs(query_data[0].result.min_distance) < DELTA);
}
......@@ -1008,7 +1004,7 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s
if(exhaustive)
{
for(size_t i = 1; i < managers.size(); ++i)
ASSERT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts());
}
else
{
......@@ -1017,10 +1013,10 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s
self_res[i] = (self_data[i].result.numContacts() > 0);
for(size_t i = 1; i < self_res.size(); ++i)
ASSERT_TRUE(self_res[0] == self_res[i]);
BOOST_CHECK(self_res[0] == self_res[i]);
for(size_t i = 1; i < managers.size(); ++i)
ASSERT_TRUE(self_data[i].result.numContacts() == self_data[0].result.numContacts());
BOOST_CHECK(self_data[i].result.numContacts() == self_data[0].result.numContacts());
}
......@@ -1049,7 +1045,7 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s
if(exhaustive)
{
for(size_t j = 1; j < managers.size(); ++j)
ASSERT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts());
}
else
{
......@@ -1057,10 +1053,10 @@ void broad_phase_update_collision_test(double env_scale, std::size_t env_size, s
for(size_t j = 0; j < query_res.size(); ++j)
query_res[j] = (query_data[j].result.numContacts() > 0);
for(size_t j = 1; j < query_res.size(); ++j)
ASSERT_TRUE(query_res[0] == query_res[j]);
BOOST_CHECK(query_res[0] == query_res[j]);
for(size_t j = 1; j < managers.size(); ++j)
ASSERT_TRUE(query_data[j].result.numContacts() == query_data[0].result.numContacts());
BOOST_CHECK(query_data[j].result.numContacts() == query_data[0].result.numContacts());
}
}
......
This diff is collapsed.
......@@ -34,12 +34,16 @@
/** \author Jia Pan */
#define BOOST_TEST_MODULE "FCL_DISTANCE"
#include <boost/test/unit_test.hpp>
#include "fcl/traversal/traversal_node_bvhs.h"
#include "fcl/traversal/traversal_node_setup.h"
#include "fcl/collision_node.h"
#include "test_fcl_utility.h"
#include <gtest/gtest.h>
#include <boost/timer.hpp>
#include "fcl_resources/config.h"
#include <boost/filesystem.hpp>
using namespace fcl;
......@@ -77,12 +81,13 @@ inline bool nearlyEqual(const Vec3f& a, const Vec3f& b)
}
TEST(collision_core, mesh_distance)
BOOST_AUTO_TEST_CASE(mesh_distance)
{
std::vector<Vec3f> p1, p2;
std::vector<Triangle> t1, t2;
loadOBJFile("../test/env.obj", p1, t1);
loadOBJFile("../test/rob.obj", p2, 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);
std::vector<Transform3f> transforms; // t0
FCL_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
......@@ -106,181 +111,181 @@ TEST(collision_core, mesh_distance)
distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<RSS, MeshDistanceTraversalNodeRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<kIOS, MeshDistanceTraversalNodekIOS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 20, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, 20, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test_Oriented<OBBRSS, MeshDistanceTraversalNodeOBBRSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 20, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
BOOST_CHECK(fabs(res.distance - res_now.distance) < DELTA);
BOOST_CHECK(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
distance_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.