Commit 33ffb691 authored by Jeongseok Lee's avatar Jeongseok Lee
Browse files

Merge remote-tracking branch 'upstream/master' into fix_boxbox_collision

parents b3fe73dd bd711b29
language: cpp
os:
- linux
- osx
compiler:
- gcc
- clang
env:
- BUILD_TYPE=Debug
- BUILD_TYPE=Release
matrix:
exclude:
- os: osx
compiler: gcc
install:
# Install dependencies for FCL
- if [ "$TRAVIS_OS_NAME" = "linux" ]; then 'ci/install_linux.sh' ; fi
- if [ "$TRAVIS_OS_NAME" = "osx" ]; then 'ci/install_osx.sh' ; fi
script:
# Create build directory
- mkdir build
- cd build
# Configure
- cmake -DCMAKE_BUILD_TYPE=$BUILD_TYPE ..
# Build
- make -j4
# Run unit tests
- make test
# Make sure we can install with no issues
- sudo make -j4 install
......@@ -6,6 +6,19 @@ if (NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# Set build type variable
set(FCL_BUILD_TYPE_RELEASE FALSE)
set(FCL_BUILD_TYPE_DEBUG FALSE)
string(TOUPPER ${CMAKE_BUILD_TYPE} CMAKE_BUILD_TYPE_UPPERCASE)
if("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "RELEASE")
set(FCL_BUILD_TYPE_RELEASE TRUE)
elseif("${CMAKE_BUILD_TYPE_UPPERCASE}" STREQUAL "DEBUG")
set(FCL_BUILD_TYPE_DEBUG TRUE)
else()
message(STATUS "CMAKE_BUILD_TYPE ${CMAKE_BUILD_TYPE} unknown. Valid options are: Debug Release")
endif()
# This shouldn't be necessary, but there has been trouble
# with MSVC being set off, but MSVCXX ON.
if(MSVC OR MSVC90 OR MSVC10)
......@@ -92,6 +105,7 @@ add_definitions(-DBOOST_TEST_DYN_LINK)
# FCL's own include dir should be at the front of the include path
include_directories(BEFORE "include")
include_directories("${CMAKE_CURRENT_BINARY_DIR}/include")
if(PKG_CONFIG_FOUND)
pkg_check_modules(CCD ccd)
......@@ -120,15 +134,16 @@ link_directories(${CCD_LIBRARY_DIRS})
add_subdirectory(include/fcl)
add_subdirectory(src)
set(pkg_conf_file "${CMAKE_CURRENT_SOURCE_DIR}/fcl.pc")
configure_file("${pkg_conf_file}.in" "${pkg_conf_file}" @ONLY)
set(pkg_conf_file_in "${CMAKE_CURRENT_SOURCE_DIR}/fcl.pc.in")
set(pkg_conf_file_out "${CMAKE_CURRENT_BINARY_DIR}/fcl.pc")
configure_file("${pkg_conf_file_in}" "${pkg_conf_file_out}" @ONLY)
install(DIRECTORY include/ DESTINATION include
FILES_MATCHING PATTERN "*.h" PATTERN "*.hxx"
PATTERN ".DS_Store" EXCLUDE
)
install(FILES "${pkg_conf_file}" DESTINATION lib/pkgconfig/ COMPONENT pkgconfig)
install(FILES "${pkg_conf_file_out}" DESTINATION lib/pkgconfig/ COMPONENT pkgconfig)
enable_testing()
......
## FCL -- The Flexible Collision Library
## FCL -- The Flexible Collision Library [![Build Status](https://travis-ci.org/flexible-collision-library/fcl.svg)](https://travis-ci.org/flexible-collision-library/fcl)
FCL is a library for performing three types of proximity queries on a pair of geometric models composed of triangles.
- Collision detection: detecting whether the two models overlap, and optionally, all of the triangles that overlap.
......
sudo add-apt-repository --yes ppa:libccd-debs/ppa
sudo apt-get -qq update
########################
# Mendatory dependencies
########################
sudo apt-get -qq --yes --force-yes install cmake
sudo apt-get -qq --yes --force-yes install libboost-all-dev
sudo apt-get -qq --yes --force-yes install libccd-dev
########################
# Optional dependencies
########################
sudo apt-get -qq --yes --force-yes install libflann-dev
# Octomap
git clone https://github.com/OctoMap/octomap
cd octomap
git checkout tags/v1.6.8
mkdir build
cd build
cmake ..
make
sudo make install
brew tap homebrew/science
brew install git
brew install cmake
brew install boost
brew install libccd
file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" VERSION_DIR)
configure_file("${VERSION_DIR}/config.h.in" "${VERSION_DIR}/config.h")
file(GLOB_RECURSE HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/*.h)
file(GLOB_RECURSE CONFIGURED_HEADERS ${CMAKE_CURRENT_BINARY_DIR}/*.h)
set(FCL_HEADERS ${HEADERS} ${CONFIGURED_HEADERS} PARENT_SCOPE)
file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}" FCL_CONFIG_IN_DIR)
file(TO_NATIVE_PATH "${CMAKE_CURRENT_BINARY_DIR}" FCL_CONFIG_OUT_DIR)
configure_file("${FCL_CONFIG_IN_DIR}/config.h.in" "${FCL_CONFIG_OUT_DIR}/config.h")
install(FILES "${FCL_CONFIG_OUT_DIR}/config.h" DESTINATION include/fcl)
......@@ -45,4 +45,7 @@
#cmakedefine01 FCL_HAVE_FLANN
#cmakedefine01 FCL_HAVE_TINYXML
#cmakedefine01 FCL_BUILD_TYPE_DEBUG
#cmakedefine01 FCL_BUILD_TYPE_RELEASE
#endif
......@@ -175,8 +175,8 @@ public:
{
return
data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 &&
data (0,0) == 0 && data (0,1) == 1 && data (0,2) == 0 &&
data (0,0) == 0 && data (0,1) == 0 && data (0,2) == 1;
data (1,0) == 0 && data (1,1) == 1 && data (1,2) == 0 &&
data (2,0) == 0 && data (2,1) == 0 && data (2,2) == 1;
}
inline void setZero()
......
This diff is collapsed.
file(GLOB_RECURSE FCL_SOURCE_CODE ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp)
if(FCL_STATIC_LIBRARY)
add_library(${PROJECT_NAME} STATIC ${FCL_SOURCE_CODE})
add_library(${PROJECT_NAME} STATIC ${FCL_HEADERS} ${FCL_SOURCE_CODE})
else()
add_library(${PROJECT_NAME} SHARED ${FCL_SOURCE_CODE})
add_library(${PROJECT_NAME} SHARED ${FCL_HEADERS} ${FCL_SOURCE_CODE})
endif()
target_link_libraries(${PROJECT_NAME} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES} ${Boost_LIBRARIES})
......
This diff is collapsed.
......@@ -10,14 +10,17 @@ macro(add_fcl_test test_name)
endmacro(add_fcl_test)
# configure location of resources
file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/fcl_resources" TEST_RESOURCES_DIR)
file(TO_NATIVE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/fcl_resources" TEST_RESOURCES_SRC_DIR)
file(TO_NATIVE_PATH "${CMAKE_CURRENT_BINARY_DIR}/fcl_resources" TEST_RESOURCES_BIN_DIR)
if(WIN32)
# Correct directory separator for Windows
string(REPLACE "\\" "\\\\" TEST_RESOURCES_DIR ${TEST_RESOURCES_DIR})
string(REPLACE "\\" "\\\\" TEST_RESOURCES_SRC_DIR ${TEST_RESOURCES_SRC_DIR})
string(REPLACE "\\" "\\\\" TEST_RESOURCES_BIN_DIR ${TEST_RESOURCES_BIN_DIR})
endif(WIN32)
configure_file("${TEST_RESOURCES_DIR}/config.h.in" "${TEST_RESOURCES_DIR}/config.h")
configure_file("${TEST_RESOURCES_SRC_DIR}/config.h.in" "${TEST_RESOURCES_BIN_DIR}/config.h")
include_directories(.)
include_directories("${CMAKE_CURRENT_BINARY_DIR}")
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)
......
......@@ -37,5 +37,5 @@
#ifndef FCL_TEST_RESOURCES_CONFIG_
#define FCL_TEST_RESOURCES_CONFIG_
#define TEST_RESOURCES_DIR "@TEST_RESOURCES_DIR@"
#define TEST_RESOURCES_DIR "@TEST_RESOURCES_SRC_DIR@"
#endif
......@@ -38,6 +38,7 @@
#define BOOST_TEST_MODULE "FCL_BROADPHASE"
#include <boost/test/unit_test.hpp>
#include "fcl/config.h"
#include "fcl/broadphase/broadphase.h"
#include "fcl/shape/geometric_shape_to_BVH_model.h"
#include "fcl/math/transform.h"
......@@ -151,10 +152,17 @@ BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_self_distance)
/// check broad phase collision and self collision, only return collision or not
BOOST_AUTO_TEST_CASE(test_core_bf_broad_phase_collision_binary)
{
#ifdef FCL_BUILD_TYPE_DEBUG
broad_phase_collision_test(2000, 10, 100, 1, false);
broad_phase_collision_test(2000, 100, 100, 1, false);
broad_phase_collision_test(2000, 10, 100, 1, true);
broad_phase_collision_test(2000, 100, 100, 1, true);
#else
broad_phase_collision_test(2000, 100, 1000, 1, false);
broad_phase_collision_test(2000, 1000, 1000, 1, false);
broad_phase_collision_test(2000, 100, 1000, 1, true);
broad_phase_collision_test(2000, 1000, 1000, 1, true);
#endif
}
/// check broad phase collision and self collision, return 10 contacts
......@@ -174,24 +182,41 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh_binary)
/// check broad phase update, in mesh, return 10 contacts
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh)
{
#ifdef FCL_BUILD_TYPE_DEBUG
broad_phase_update_collision_test(200, 10, 100, 10, false, true);
broad_phase_update_collision_test(200, 100, 100, 10, false, true);
#else
broad_phase_update_collision_test(2000, 100, 1000, 10, false, true);
broad_phase_update_collision_test(2000, 1000, 1000, 10, false, true);
#endif
}
/// check broad phase update, in mesh, exhaustive
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_update_collision_mesh_exhaustive)
{
#ifdef FCL_BUILD_TYPE_DEBUG
broad_phase_update_collision_test(2000, 10, 100, 1, true, true);
broad_phase_update_collision_test(2000, 100, 100, 1, true, true);
#else
broad_phase_update_collision_test(2000, 100, 1000, 1, true, true);
broad_phase_update_collision_test(2000, 1000, 1000, 1, true, true);
#endif
}
/// check broad phase distance
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_distance_mesh)
{
#ifdef FCL_BUILD_TYPE_DEBUG
broad_phase_distance_test(200, 10, 10, true);
broad_phase_distance_test(200, 100, 10, true);
broad_phase_distance_test(2000, 10, 10, true);
broad_phase_distance_test(2000, 100, 10, true);
#else
broad_phase_distance_test(200, 100, 100, true);
broad_phase_distance_test(200, 1000, 100, true);
broad_phase_distance_test(2000, 100, 100, true);
broad_phase_distance_test(2000, 1000, 100, true);
#endif
}
/// check broad phase self distance
......@@ -205,22 +230,37 @@ BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_self_distance_mesh)
/// check broad phase collision and self collision, return only collision or not, in mesh
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_binary)
{
#ifdef FCL_BUILD_TYPE_DEBUG
broad_phase_collision_test(2000, 10, 100, 1, false, true);
broad_phase_collision_test(2000, 100, 100, 1, false, true);
#else
broad_phase_collision_test(2000, 100, 1000, 1, false, true);
broad_phase_collision_test(2000, 1000, 1000, 1, false, true);
#endif
}
/// check broad phase collision and self collision, return 10 contacts, in mesh
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh)
{
#ifdef FCL_BUILD_TYPE_DEBUG
broad_phase_collision_test(2000, 10, 100, 10, false, true);
broad_phase_collision_test(2000, 100, 100, 10, false, true);
#else
broad_phase_collision_test(2000, 100, 1000, 10, false, true);
broad_phase_collision_test(2000, 1000, 1000, 10, false, true);
#endif
}
/// check broad phase collision and self collision, exhaustive, in mesh
BOOST_AUTO_TEST_CASE(test_core_mesh_bf_broad_phase_collision_mesh_exhaustive)
{
#ifdef FCL_BUILD_TYPE_DEBUG
broad_phase_collision_test(2000, 10, 100, 1, true, true);
broad_phase_collision_test(2000, 100, 100, 1, true, true);
#else
broad_phase_collision_test(2000, 100, 1000, 1, true, true);
broad_phase_collision_test(2000, 1000, 1000, 1, true, true);
#endif
}
void generateEnvironments(std::vector<CollisionObject*>& env, double env_scale, std::size_t n)
......
......@@ -90,7 +90,8 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
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);
// Disabled broken test lines. Please see #25.
// 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);
......
......@@ -72,10 +72,12 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
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);
// Disabled broken test lines. Please see #25.
// 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);
// Disabled broken test lines. Please see #25.
// BOOST_CHECK_CLOSE (o2 [1], 0.8, 1e-4);
// BOOST_CHECK_CLOSE (o2 [2], 1.5, 1e-4);
}
......@@ -115,9 +115,10 @@ BOOST_AUTO_TEST_CASE(front_list)
for(std::size_t i = 0; i < transforms.size(); ++i)
{
res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
BOOST_CHECK(res == res2);
// Disabled broken test lines. Please see #25.
// res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, verbose);
// res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
// BOOST_CHECK(res == res2);
res = collide_Test<RSS>(transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
res2 = collide_front_list_Test<RSS>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, false, verbose);
BOOST_CHECK(res == res2);
......
......@@ -2752,3 +2752,177 @@ BOOST_AUTO_TEST_CASE(conecone)
template<typename S1, typename S2>
void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distance)
{
Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
Vec3f contactA;
Vec3f contactB;
FCL_REAL depthA;
FCL_REAL depthB;
Vec3f normalA;
Vec3f normalB;
bool resA;
bool resB;
const double tol = 1e-6;
resA = solver1.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
resB = solver1.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK(contactA.equal(contactB, tol)); // contact point should be same
BOOST_CHECK(normalA.equal(-normalB, tol)); // normal should be opposite
BOOST_CHECK_CLOSE(depthA, depthB, tol); // penetration depth should be same
resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
resB = solver2.shapeIntersect(s2, tf2, s1, tf1, &contactB, &depthB, &normalB);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK(contactA.equal(contactB, tol));
BOOST_CHECK(normalA.equal(-normalB, tol));
BOOST_CHECK_CLOSE(depthA, depthB, tol);
}
BOOST_AUTO_TEST_CASE(reversibleShapeIntersection_allshapes)
{
// This test check whether a shape intersection algorithm is called for the
// reverse case as well. For example, if FCL has sphere-capsule intersection
// algorithm, then this algorithm should be called for capsule-sphere case.
// Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
Box box(10, 10, 10);
Sphere sphere(5);
Capsule capsule(5, 10);
Cone cone(5, 10);
Cylinder cylinder(5, 10);
Plane plane(Vec3f(), 0.0);
Halfspace halfspace(Vec3f(), 0.0);
// Use sufficiently short distance so that all the primitive shapes can intersect
FCL_REAL distance = 5.0;
// If new shape intersection algorithm is added for two distinct primitive
// shapes, uncomment associated lines. For example, box-sphere intersection
// algorithm is added, then uncomment box-sphere.
// testReversibleShapeIntersection(box, sphere, distance);
// testReversibleShapeIntersection(box, capsule, distance);
// testReversibleShapeIntersection(box, cone, distance);
// testReversibleShapeIntersection(box, cylinder, distance);
testReversibleShapeIntersection(box, plane, distance);
testReversibleShapeIntersection(box, halfspace, distance);
testReversibleShapeIntersection(sphere, capsule, distance);
// testReversibleShapeIntersection(sphere, cone, distance);
// testReversibleShapeIntersection(sphere, cylinder, distance);
testReversibleShapeIntersection(sphere, plane, distance);
testReversibleShapeIntersection(sphere, halfspace, distance);
// testReversibleShapeIntersection(capsule, cone, distance);
// testReversibleShapeIntersection(capsule, cylinder, distance);
testReversibleShapeIntersection(capsule, plane, distance);
testReversibleShapeIntersection(capsule, halfspace, distance);
// testReversibleShapeIntersection(cone, cylinder, distance);
testReversibleShapeIntersection(cone, plane, distance);
testReversibleShapeIntersection(cone, halfspace, distance);
testReversibleShapeIntersection(cylinder, plane, distance);
testReversibleShapeIntersection(cylinder, halfspace, distance);
testReversibleShapeIntersection(plane, halfspace, distance);
}
template<typename S1, typename S2>
void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance)
{
Transform3f tf1(Vec3f(-0.5 * distance, 0.0, 0.0));
Transform3f tf2(Vec3f(+0.5 * distance, 0.0, 0.0));
FCL_REAL distA;
FCL_REAL distB;
Vec3f p1A;
Vec3f p1B;
Vec3f p2A;
Vec3f p2B;
bool resA;
bool resB;
const double tol = 1e-6;
resA = solver1.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
resB = solver1.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same
BOOST_CHECK(p1A.equal(p2B, tol)); // closest points should in reverse order
BOOST_CHECK(p2A.equal(p1B, tol));
resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK_CLOSE(distA, distB, tol);
BOOST_CHECK(p1A.equal(p2B, tol));
BOOST_CHECK(p2A.equal(p1B, tol));
}
BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes)
{
// This test check whether a shape distance algorithm is called for the
// reverse case as well. For example, if FCL has sphere-capsule distance
// algorithm, then this algorithm should be called for capsule-sphere case.
// Prepare all kinds of primitive shapes (7) -- box, sphere, capsule, cone, cylinder, plane, halfspace
Box box(10, 10, 10);
Sphere sphere(5);
Capsule capsule(5, 10);
Cone cone(5, 10);
Cylinder cylinder(5, 10);
Plane plane(Vec3f(), 0.0);
Halfspace halfspace(Vec3f(), 0.0);
// Use sufficiently long distance so that all the primitive shapes CANNOT intersect
FCL_REAL distance = 15.0;
// If new shape distance algorithm is added for two distinct primitive
// shapes, uncomment associated lines. For example, box-sphere intersection
// algorithm is added, then uncomment box-sphere.
// testReversibleShapeDistance(box, sphere, distance);
// testReversibleShapeDistance(box, capsule, distance);
// testReversibleShapeDistance(box, cone, distance);
// testReversibleShapeDistance(box, cylinder, distance);
// testReversibleShapeDistance(box, plane, distance);
// testReversibleShapeDistance(box, halfspace, distance);
testReversibleShapeDistance(sphere, capsule, distance);
// testReversibleShapeDistance(sphere, cone, distance);
// testReversibleShapeDistance(sphere, cylinder, distance);
// testReversibleShapeDistance(sphere, plane, distance);
// testReversibleShapeDistance(sphere, halfspace, distance);
// testReversibleShapeDistance(capsule, cone, distance);
// testReversibleShapeDistance(capsule, cylinder, distance);
// testReversibleShapeDistance(capsule, plane, distance);
// testReversibleShapeDistance(capsule, halfspace, distance);
// testReversibleShapeDistance(cone, cylinder, distance);
// testReversibleShapeDistance(cone, plane, distance);
// testReversibleShapeDistance(cone, halfspace, distance);
// testReversibleShapeDistance(cylinder, plane, distance);
// testReversibleShapeDistance(cylinder, halfspace, distance);
// testReversibleShapeDistance(plane, halfspace, distance);
}
......@@ -37,6 +37,7 @@
#define BOOST_TEST_MODULE "FCL_OCTOMAP"
#include <boost/test/unit_test.hpp>
#include "fcl/config.h"
#include "fcl/octree.h"
#include "fcl/traversal/traversal_node_octree.h"
#include "fcl/broadphase/broadphase.h"
......@@ -117,18 +118,32 @@ BOOST_AUTO_TEST_CASE(test_octomap_collision)
BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh)
{
#ifdef FCL_BUILD_TYPE_DEBUG
octomap_collision_test(200, 10, false, 10, true, true);
octomap_collision_test(200, 100, false, 10, true, true);
octomap_collision_test(200, 10, true, 1, true, true);
octomap_collision_test(200, 100, true, 1, true, true);
#else
octomap_collision_test(200, 100, false, 10, true, true);
octomap_collision_test(200, 1000, false, 10, true, true);
octomap_collision_test(200, 100, true, 1, true, true);
octomap_collision_test(200, 1000, true, 1, true, true);
#endif
}
BOOST_AUTO_TEST_CASE(test_octomap_collision_mesh_octomap_box)
{
#ifdef FCL_BUILD_TYPE_DEBUG
octomap_collision_test(200, 10, false, 10, true, false);
octomap_collision_test(200, 100, false, 10, true, false);
octomap_collision_test(200, 10, true, 1, true, false);
octomap_collision_test(200, 100, true, 1, true, false);
#else
octomap_collision_test(200, 100, false, 10, true, false);
octomap_collision_test(200, 1000, false, 10, true, false);
octomap_collision_test(200, 100, true, 1, true, false);
octomap_collision_test(200, 1000, true, 1, true, false);
#endif
}
BOOST_AUTO_TEST_CASE(test_octomap_distance)
......@@ -139,17 +154,26 @@ BOOST_AUTO_TEST_CASE(test_octomap_distance)
BOOST_AUTO_TEST_CASE(test_octomap_distance_mesh)
{
#ifdef FCL_BUILD_TYPE_DEBUG
octomap_distance_test(200, 5, true, true);
octomap_distance_test(200, 50, true, true);
#else
octomap_distance_test(200, 100, true, true);
octomap_distance_test(200, 1000, true, true);
#endif
}
BOOST_AUTO_TEST_CASE(test_octomap_distance_mesh_octomap_box)
{
#ifdef FCL_BUILD_TYPE_DEBUG
octomap_distance_test(200, 10, true, false);
octomap_distance_test(200, 100, true, false);
#else
octomap_distance_test(200, 100, true, false);
octomap_distance_test(200, 1000, true, false);
#endif
}
BOOST_AUTO_TEST_CASE(test_octomap_bvh_obb_collision_obb)
{
octomap_collision_test_BVH<OBB>(5, false);
......@@ -168,7 +192,11 @@ BOOST_AUTO_TEST_CASE(test_octomap_bvh_obb_d_distance_obb)