From c45c79a7e887256d6e76ed6b3a464701b7e51dfa Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Fri, 3 Aug 2012 05:11:48 +0000
Subject: [PATCH] add necessary documentation and change according to code
review
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@152 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 12 +-
trunk/fcl/include/fcl/BV.h | 2 +-
trunk/fcl/include/fcl/BV/AABB.h | 2 +-
trunk/fcl/include/fcl/BV/OBB.h | 4 +-
trunk/fcl/include/fcl/BV/OBBRSS.h | 4 +-
trunk/fcl/include/fcl/BV/RSS.h | 4 +-
trunk/fcl/include/fcl/BV/kDOP.h | 2 +-
trunk/fcl/include/fcl/BV/kIOS.h | 3 -
trunk/fcl/include/fcl/BVH_model.h | 1 -
trunk/fcl/include/fcl/BVH_utility.h | 5 -
trunk/fcl/include/fcl/BV_fitter.h | 3 -
trunk/fcl/include/fcl/BV_node.h | 4 +-
trunk/fcl/include/fcl/BV_splitter.h | 1 -
.../include/fcl/broadphase/hierarchy_tree.h | 3 +-
.../fcl/{ => ccd}/conservative_advancement.h | 3 +-
trunk/fcl/include/fcl/ccd/interval_matrix.h | 2 +-
trunk/fcl/include/fcl/ccd/interval_vector.h | 2 +-
trunk/fcl/include/fcl/{ => ccd}/motion.h | 6 +-
trunk/fcl/include/fcl/{ => ccd}/motion_base.h | 6 +-
trunk/fcl/include/fcl/ccd/taylor_matrix.h | 2 +-
trunk/fcl/include/fcl/collision.h | 2 +-
trunk/fcl/include/fcl/collision_data.h | 77 +-
trunk/fcl/include/fcl/collision_func_matrix.h | 7 +
trunk/fcl/include/fcl/collision_node.h | 2 +-
trunk/fcl/include/fcl/collision_object.h | 2 +-
trunk/fcl/include/fcl/distance.h | 2 +
trunk/fcl/include/fcl/distance_func_matrix.h | 6 +
.../fcl/geometric_shape_to_BVH_model.h | 184 +---
trunk/fcl/include/fcl/geometric_shapes.h | 92 +-
.../include/fcl/geometric_shapes_utility.h | 9 +-
trunk/fcl/include/fcl/input_represent.h | 178 ----
trunk/fcl/include/fcl/intersect.h | 183 ++--
.../fcl/include/fcl/{ => math}/math_details.h | 0
trunk/fcl/include/fcl/{ => math}/matrix_3f.h | 5 +-
trunk/fcl/include/fcl/{ => math}/transform.h | 140 ++--
trunk/fcl/include/fcl/{ => math}/vec_3f.h | 13 +-
trunk/fcl/include/fcl/narrowphase/gjk.h | 69 +-
.../fcl/include/fcl/narrowphase/gjk_libccd.h | 35 +-
.../fcl/include/fcl/narrowphase/narrowphase.h | 74 +-
trunk/fcl/include/fcl/octree.h | 26 +-
trunk/fcl/include/fcl/profile.h | 149 ++--
.../fcl/{ => simd}/math_simd_details.h | 0
trunk/fcl/include/fcl/{ => simd}/simd.h | 2 +-
.../include/fcl/{ => simd}/simd_intersect.h | 2 +-
trunk/fcl/include/fcl/traversal_node_base.h | 41 +-
.../include/fcl/traversal_node_bvh_shape.h | 66 +-
trunk/fcl/include/fcl/traversal_node_bvhs.h | 483 ++---------
trunk/fcl/include/fcl/traversal_node_octree.h | 53 +-
...{simple_setup.h => traversal_node_setup.h} | 350 ++------
trunk/fcl/include/fcl/traversal_node_shapes.h | 11 +-
trunk/fcl/include/fcl/traversal_recurse.h | 12 +-
trunk/fcl/manifest.xml | 1 -
trunk/fcl/src/BV/OBB.cpp | 2 +-
trunk/fcl/src/BV/kIOS.cpp | 2 +-
trunk/fcl/src/BVH_utility.cpp | 91 --
.../{ => ccd}/conservative_advancement.cpp | 7 +-
trunk/fcl/src/{ => ccd}/motion.cpp | 2 +-
trunk/fcl/src/collision_data.cpp | 52 ++
trunk/fcl/src/collision_func_matrix.cpp | 32 +-
trunk/fcl/src/distance_func_matrix.cpp | 16 +-
trunk/fcl/src/intersect.cpp | 786 ++----------------
trunk/fcl/src/{ => math}/transform.cpp | 33 +-
trunk/fcl/src/profile.cpp | 33 +-
trunk/fcl/src/traversal_node_bvhs.cpp | 192 +----
...ple_setup.cpp => traversal_node_setup.cpp} | 139 +---
65 files changed, 975 insertions(+), 2759 deletions(-)
rename trunk/fcl/include/fcl/{ => ccd}/conservative_advancement.h (97%)
rename trunk/fcl/include/fcl/{ => ccd}/motion.h (99%)
rename trunk/fcl/include/fcl/{ => ccd}/motion_base.h (97%)
delete mode 100644 trunk/fcl/include/fcl/input_represent.h
rename trunk/fcl/include/fcl/{ => math}/math_details.h (100%)
rename trunk/fcl/include/fcl/{ => math}/matrix_3f.h (97%)
rename trunk/fcl/include/fcl/{ => math}/transform.h (63%)
rename trunk/fcl/include/fcl/{ => math}/vec_3f.h (95%)
rename trunk/fcl/include/fcl/{ => simd}/math_simd_details.h (100%)
rename trunk/fcl/include/fcl/{ => simd}/simd.h (97%)
rename trunk/fcl/include/fcl/{ => simd}/simd_intersect.h (99%)
rename trunk/fcl/include/fcl/{simple_setup.h => traversal_node_setup.h} (75%)
rename trunk/fcl/src/{ => ccd}/conservative_advancement.cpp (97%)
rename trunk/fcl/src/{ => ccd}/motion.cpp (99%)
create mode 100644 trunk/fcl/src/collision_data.cpp
rename trunk/fcl/src/{ => math}/transform.cpp (91%)
rename trunk/fcl/src/{simple_setup.cpp => traversal_node_setup.cpp} (54%)
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 79fc8fd5..b376bc74 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -10,10 +10,6 @@ project(fcl CXX C)
#set(ROS_BUILD_TYPE RelWithDebInfo)
set(CMAKE_BUILD_TYPE Release)
-set(FCL_VERSION "0.1.1")
-set(PKG_DESC "Fast Collision Library")
-set(PKG_EXTERNAL_DEPS "ccd flann octomap")
-
set(CMAKE_SKIP_BUILD_RPATH FALSE)
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
@@ -27,10 +23,6 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
include_directories("include")
find_package(PkgConfig)
-pkg_check_modules(FLANN REQUIRED flann)
-include_directories(${FLANN_INCLUDE_DIRS})
-link_directories(${FLANN_LIBRARY_DIRS})
-
pkg_check_modules(CCD REQUIRED ccd)
include_directories(${CCD_INCLUDE_DIRS})
link_directories(${CCD_LIBRARY_DIRS})
@@ -41,10 +33,10 @@ link_directories(${OCTOMAP_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
-add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/BV/kDOP.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broadphase/broadphase_bruteforce.cpp src/broadphase/broadphase_spatialhash.cpp src/broadphase/broadphase_SaP.cpp src/broadphase/broadphase_SSaP.cpp src/broadphase/broadphase_interval_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree_array.cpp src/collision.cpp src/collision_func_matrix.cpp src/broadphase/interval_tree.cpp src/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/broadphase/hierarchy_tree.cpp)
+add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/BV/kDOP.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/ccd/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/math/transform.cpp src/traversal_node_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broadphase/broadphase_bruteforce.cpp src/broadphase/broadphase_spatialhash.cpp src/broadphase/broadphase_SaP.cpp src/broadphase/broadphase_SSaP.cpp src/broadphase/broadphase_interval_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree.cpp src/broadphase/broadphase_dynamic_AABB_tree_array.cpp src/collision.cpp src/collision_func_matrix.cpp src/broadphase/interval_tree.cpp src/ccd/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/broadphase/hierarchy_tree.cpp src/profile.cpp src/collision_data.cpp)
-target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES})
set(pkg_conf_file "${CMAKE_CURRENT_SOURCE_DIR}/fcl.pc")
configure_file("${pkg_conf_file}.in" "${pkg_conf_file}" @ONLY)
diff --git a/trunk/fcl/include/fcl/BV.h b/trunk/fcl/include/fcl/BV.h
index 549e7fa8..4c236b0b 100644
--- a/trunk/fcl/include/fcl/BV.h
+++ b/trunk/fcl/include/fcl/BV.h
@@ -44,7 +44,7 @@
#include "fcl/BV/RSS.h"
#include "fcl/BV/OBBRSS.h"
#include "fcl/BV/kIOS.h"
-#include "fcl/transform.h"
+#include "fcl/math/transform.h"
/** \brief Main namespace */
namespace fcl
diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/trunk/fcl/include/fcl/BV/AABB.h
index cbfe1914..558af595 100644
--- a/trunk/fcl/include/fcl/BV/AABB.h
+++ b/trunk/fcl/include/fcl/BV/AABB.h
@@ -39,7 +39,7 @@
#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
+#include "fcl/math/vec_3f.h"
namespace fcl
{
diff --git a/trunk/fcl/include/fcl/BV/OBB.h b/trunk/fcl/include/fcl/BV/OBB.h
index 92d0471e..7138b9ad 100644
--- a/trunk/fcl/include/fcl/BV/OBB.h
+++ b/trunk/fcl/include/fcl/BV/OBB.h
@@ -38,8 +38,8 @@
#define FCL_OBB_H
#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-#include "fcl/matrix_3f.h"
+#include "fcl/math/vec_3f.h"
+#include "fcl/math/matrix_3f.h"
namespace fcl
{
diff --git a/trunk/fcl/include/fcl/BV/OBBRSS.h b/trunk/fcl/include/fcl/BV/OBBRSS.h
index 8925ac7d..7474f2b8 100644
--- a/trunk/fcl/include/fcl/BV/OBBRSS.h
+++ b/trunk/fcl/include/fcl/BV/OBBRSS.h
@@ -38,8 +38,8 @@
#define FCL_OBBRSS_H
#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-#include "fcl/matrix_3f.h"
+#include "fcl/math/vec_3f.h"
+#include "fcl/math/matrix_3f.h"
#include "fcl/BV/OBB.h"
#include "fcl/BV/RSS.h"
diff --git a/trunk/fcl/include/fcl/BV/RSS.h b/trunk/fcl/include/fcl/BV/RSS.h
index f0293eeb..f1e97dfa 100644
--- a/trunk/fcl/include/fcl/BV/RSS.h
+++ b/trunk/fcl/include/fcl/BV/RSS.h
@@ -38,8 +38,8 @@
#define FCL_RSS_H
#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-#include "fcl/matrix_3f.h"
+#include "fcl/math/vec_3f.h"
+#include "fcl/math/matrix_3f.h"
#include <boost/math/constants/constants.hpp>
namespace fcl
diff --git a/trunk/fcl/include/fcl/BV/kDOP.h b/trunk/fcl/include/fcl/BV/kDOP.h
index 660cdfbb..ecc5f900 100644
--- a/trunk/fcl/include/fcl/BV/kDOP.h
+++ b/trunk/fcl/include/fcl/BV/kDOP.h
@@ -38,7 +38,7 @@
#define FCL_KDOP_H
#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
+#include "fcl/math/vec_3f.h"
namespace fcl
{
diff --git a/trunk/fcl/include/fcl/BV/kIOS.h b/trunk/fcl/include/fcl/BV/kIOS.h
index b39dd3c6..8f81dfe3 100644
--- a/trunk/fcl/include/fcl/BV/kIOS.h
+++ b/trunk/fcl/include/fcl/BV/kIOS.h
@@ -37,9 +37,6 @@
#ifndef FCL_KIOS_H
#define FCL_KIOS_H
-#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-#include "fcl/matrix_3f.h"
#include "fcl/BV/OBB.h"
diff --git a/trunk/fcl/include/fcl/BVH_model.h b/trunk/fcl/include/fcl/BVH_model.h
index e4652272..63e91f16 100644
--- a/trunk/fcl/include/fcl/BVH_model.h
+++ b/trunk/fcl/include/fcl/BVH_model.h
@@ -41,7 +41,6 @@
#include "fcl/BVH_internal.h"
#include "fcl/BV.h"
#include "fcl/BV_node.h"
-#include "fcl/vec_3f.h"
#include "fcl/BV_splitter.h"
#include "fcl/BV_fitter.h"
#include <vector>
diff --git a/trunk/fcl/include/fcl/BVH_utility.h b/trunk/fcl/include/fcl/BVH_utility.h
index 70b4d976..1025a4eb 100644
--- a/trunk/fcl/include/fcl/BVH_utility.h
+++ b/trunk/fcl/include/fcl/BVH_utility.h
@@ -38,7 +38,6 @@
#ifndef FCL_BVH_UTILITY_H
#define FCL_BVH_UTILITY_H
-#include "fcl/vec_3f.h"
#include "fcl/BVH_model.h"
@@ -77,10 +76,6 @@ void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r);
/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for RSS
void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r);
-/// @brief Estimate the variance of point clouds due to sampling procedure
-void estimateSamplingVariance(Vec3f* vertices, int num_vertices, Variance3f* ucs);
-
-
/// @brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);
diff --git a/trunk/fcl/include/fcl/BV_fitter.h b/trunk/fcl/include/fcl/BV_fitter.h
index d88788ec..d5380da3 100644
--- a/trunk/fcl/include/fcl/BV_fitter.h
+++ b/trunk/fcl/include/fcl/BV_fitter.h
@@ -40,9 +40,6 @@
#include "fcl/BVH_internal.h"
#include "fcl/data_types.h"
-#include "fcl/vec_3f.h"
-#include "fcl/BV/OBB.h"
-#include "fcl/BV/RSS.h"
#include "fcl/BV/kIOS.h"
#include "fcl/BV/OBBRSS.h"
#include <iostream>
diff --git a/trunk/fcl/include/fcl/BV_node.h b/trunk/fcl/include/fcl/BV_node.h
index 534e0619..9776d660 100644
--- a/trunk/fcl/include/fcl/BV_node.h
+++ b/trunk/fcl/include/fcl/BV_node.h
@@ -38,8 +38,8 @@
#ifndef FCL_BV_NODE_H
#define FCL_BV_NODE_H
-#include "fcl/vec_3f.h"
-#include "fcl/matrix_3f.h"
+#include "fcl/math/vec_3f.h"
+#include "fcl/math/matrix_3f.h"
#include "fcl/BV.h"
#include <iostream>
diff --git a/trunk/fcl/include/fcl/BV_splitter.h b/trunk/fcl/include/fcl/BV_splitter.h
index cde0a6e8..8f1b6094 100644
--- a/trunk/fcl/include/fcl/BV_splitter.h
+++ b/trunk/fcl/include/fcl/BV_splitter.h
@@ -40,7 +40,6 @@
#include "fcl/BVH_internal.h"
#include "fcl/data_types.h"
-#include "fcl/vec_3f.h"
#include "fcl/BV/OBB.h"
#include "fcl/BV/RSS.h"
#include "fcl/BV/kIOS.h"
diff --git a/trunk/fcl/include/fcl/broadphase/hierarchy_tree.h b/trunk/fcl/include/fcl/broadphase/hierarchy_tree.h
index 2787f1da..64dcd283 100644
--- a/trunk/fcl/include/fcl/broadphase/hierarchy_tree.h
+++ b/trunk/fcl/include/fcl/broadphase/hierarchy_tree.h
@@ -40,7 +40,6 @@
#include <vector>
#include <map>
#include "fcl/BV/AABB.h"
-#include "fcl/vec_3f.h"
#include "fcl/broadphase/morton.h"
#include <boost/bind.hpp>
#include <boost/iterator/zip_iterator.hpp>
@@ -122,7 +121,7 @@ class HierarchyTree
struct SortByMorton
{
- bool operator() (NodeType* a, NodeType* b) const
+ bool operator() (const NodeType* a, const NodeType* b) const
{
return a->code < b->code;
}
diff --git a/trunk/fcl/include/fcl/conservative_advancement.h b/trunk/fcl/include/fcl/ccd/conservative_advancement.h
similarity index 97%
rename from trunk/fcl/include/fcl/conservative_advancement.h
rename to trunk/fcl/include/fcl/ccd/conservative_advancement.h
index 5ee477c3..7f4290ce 100644
--- a/trunk/fcl/include/fcl/conservative_advancement.h
+++ b/trunk/fcl/include/fcl/ccd/conservative_advancement.h
@@ -38,10 +38,9 @@
#define FCL_CONSERVATIVE_ADVANCEMENT_H
-#include "fcl/vec_3f.h"
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
-#include "fcl/motion_base.h"
+#include "fcl/ccd/motion_base.h"
namespace fcl
diff --git a/trunk/fcl/include/fcl/ccd/interval_matrix.h b/trunk/fcl/include/fcl/ccd/interval_matrix.h
index 2a3195a5..a6fe5ab5 100644
--- a/trunk/fcl/include/fcl/ccd/interval_matrix.h
+++ b/trunk/fcl/include/fcl/ccd/interval_matrix.h
@@ -40,7 +40,7 @@
#include "fcl/ccd/interval.h"
#include "fcl/ccd/interval_vector.h"
-#include "fcl/matrix_3f.h"
+#include "fcl/math/matrix_3f.h"
namespace fcl
{
diff --git a/trunk/fcl/include/fcl/ccd/interval_vector.h b/trunk/fcl/include/fcl/ccd/interval_vector.h
index 8f2f7074..2a60dc4c 100644
--- a/trunk/fcl/include/fcl/ccd/interval_vector.h
+++ b/trunk/fcl/include/fcl/ccd/interval_vector.h
@@ -39,7 +39,7 @@
#define FCL_INTERVAL_VECTOR_H
#include "fcl/ccd/interval.h"
-#include "fcl/vec_3f.h"
+#include "fcl/math/vec_3f.h"
namespace fcl
{
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/ccd/motion.h
similarity index 99%
rename from trunk/fcl/include/fcl/motion.h
rename to trunk/fcl/include/fcl/ccd/motion.h
index 59e37e74..124bb268 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/ccd/motion.h
@@ -38,11 +38,7 @@
#ifndef FCL_MOTION_H
#define FCL_MOTION_H
-#include "fcl/vec_3f.h"
-#include "fcl/matrix_3f.h"
-#include "fcl/BV/RSS.h"
-#include "fcl/transform.h"
-#include "fcl/motion_base.h"
+#include "fcl/ccd/motion_base.h"
#include "fcl/intersect.h"
#include <iostream>
#include <vector>
diff --git a/trunk/fcl/include/fcl/motion_base.h b/trunk/fcl/include/fcl/ccd/motion_base.h
similarity index 97%
rename from trunk/fcl/include/fcl/motion_base.h
rename to trunk/fcl/include/fcl/ccd/motion_base.h
index ba48ce32..bfd61763 100644
--- a/trunk/fcl/include/fcl/motion_base.h
+++ b/trunk/fcl/include/fcl/ccd/motion_base.h
@@ -38,10 +38,10 @@
#ifndef FCL_MOTION_BASE_H
#define FCL_MOTION_BASE_H
-#include "fcl/vec_3f.h"
-#include "fcl/matrix_3f.h"
-#include "fcl/transform.h"
+
+#include "fcl/math/transform.h"
#include "fcl/BV/RSS.h"
+
namespace fcl
{
diff --git a/trunk/fcl/include/fcl/ccd/taylor_matrix.h b/trunk/fcl/include/fcl/ccd/taylor_matrix.h
index 9e513a8b..0c02a3bf 100644
--- a/trunk/fcl/include/fcl/ccd/taylor_matrix.h
+++ b/trunk/fcl/include/fcl/ccd/taylor_matrix.h
@@ -38,7 +38,7 @@
#define FCL_TAYLOR_MATRIX_H
-#include "fcl/matrix_3f.h"
+#include "fcl/math/matrix_3f.h"
#include "fcl/ccd/taylor_vector.h"
#include "fcl/ccd/interval_matrix.h"
diff --git a/trunk/fcl/include/fcl/collision.h b/trunk/fcl/include/fcl/collision.h
index d91c2169..fba8cea4 100644
--- a/trunk/fcl/include/fcl/collision.h
+++ b/trunk/fcl/include/fcl/collision.h
@@ -38,7 +38,7 @@
#ifndef FCL_COLLISION_H
#define FCL_COLLISION_H
-#include "fcl/vec_3f.h"
+#include "fcl/math/vec_3f.h"
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index ebb24a58..a3b84443 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -1,8 +1,45 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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 Jia Pan */
+
+
#ifndef FCL_COLLISION_DATA_H
#define FCL_COLLISION_DATA_H
#include "fcl/collision_object.h"
-#include "fcl/vec_3f.h"
+#include "fcl/math/vec_3f.h"
#include <vector>
#include <set>
#include <limits>
@@ -108,6 +145,8 @@ struct CostSource
}
};
+struct CollisionResult;
+
/// @brief request to the collision algorithm
struct CollisionRequest
{
@@ -133,6 +172,7 @@ struct CollisionRequest
{
}
+ bool isSatisfied(const CollisionResult& result) const;
};
/// @brief collision result
@@ -145,18 +185,11 @@ private:
/// @brief cost sources
std::set<CostSource> cost_sources;
- const CollisionRequest* request;
-
public:
CollisionResult()
{
- request = NULL;
}
- void setRequest(const CollisionRequest& request_)
- {
- request = &request_;
- }
/// @brief add one contact into result structure
inline void addContact(const Contact& c)
@@ -165,18 +198,11 @@ public:
}
/// @brief add one cost source into result structure
- inline void addCostSource(const CostSource& c)
+ inline void addCostSource(const CostSource& c, std::size_t num_max_cost_sources)
{
- if(request)
- {
- cost_sources.insert(c);
- if(cost_sources.size() > request->num_max_cost_sources)
- cost_sources.erase(cost_sources.begin());
- }
- else
- {
- cost_sources.insert(c);
- }
+ cost_sources.insert(c);
+ if(cost_sources.size() > num_max_cost_sources)
+ cost_sources.erase(cost_sources.begin());
}
/// @brief return binary collision result
@@ -228,7 +254,7 @@ public:
}
};
-
+struct DistanceResult;
/// @brief request to the distance computation
struct DistanceRequest
@@ -239,17 +265,17 @@ struct DistanceRequest
DistanceRequest(bool enable_nearest_points_ = false) : enable_nearest_points(enable_nearest_points_)
{
}
+
+ bool isSatisfied(const DistanceResult& result) const;
};
/// @brief distance result
struct DistanceResult
{
-private:
- const DistanceRequest* request;
public:
- /// @brief minimum distance between two objects
+ /// @brief minimum distance between two objects. if two objects are in collision, min_distance <= 0.
FCL_REAL min_distance;
/// @brief nearest points
@@ -282,13 +308,8 @@ public:
b1(NONE),
b2(NONE)
{
- request = NULL;
}
- void setRequest(const DistanceRequest& request_)
- {
- request = &request_;
- }
/// @brief add distance information into the result
void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/trunk/fcl/include/fcl/collision_func_matrix.h
index 57c32b12..88f809ac 100644
--- a/trunk/fcl/include/fcl/collision_func_matrix.h
+++ b/trunk/fcl/include/fcl/collision_func_matrix.h
@@ -45,11 +45,18 @@
namespace fcl
{
+/// @brief collision matrix stores the functions for collision between different types of objects and provides a uniform call interface
template<typename NarrowPhaseSolver>
struct CollisionFunctionMatrix
{
+ /// @brief the uniform call interface for collision: for collision, we need know
+ /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2;
+ /// 2. the solver for narrow phase collision, this is for the collision between geometric shapes;
+ /// 3. the request setting for collision (e.g., whether need to return normal information, whether need to compute cost);
+ /// 4. the structure to return collision result
typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result);
+ /// @brief each item in the collision matrix is a function to handle collision between objects of type1 and type2
CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
CollisionFunctionMatrix();
diff --git a/trunk/fcl/include/fcl/collision_node.h b/trunk/fcl/include/fcl/collision_node.h
index 58a7ee73..3774dcbb 100644
--- a/trunk/fcl/include/fcl/collision_node.h
+++ b/trunk/fcl/include/fcl/collision_node.h
@@ -44,7 +44,7 @@
-
+/// @brief collision and distance function on traversal nodes. these functions provide a higher level abstraction for collision functions provided in collision_func_matrix
namespace fcl
{
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index fad09584..952de096 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -39,7 +39,7 @@
#define FCL_COLLISION_OBJECT_BASE_H
#include "fcl/BV/AABB.h"
-#include "fcl/transform.h"
+#include "fcl/math/transform.h"
#include <boost/shared_ptr.hpp>
namespace fcl
diff --git a/trunk/fcl/include/fcl/distance.h b/trunk/fcl/include/fcl/distance.h
index 201e6a50..36208ec2 100644
--- a/trunk/fcl/include/fcl/distance.h
+++ b/trunk/fcl/include/fcl/distance.h
@@ -43,6 +43,8 @@
namespace fcl
{
+/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them.
+/// Return value is the minimum distance generated between the two objects.
template<typename NarrowPhaseSolver>
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
diff --git a/trunk/fcl/include/fcl/distance_func_matrix.h b/trunk/fcl/include/fcl/distance_func_matrix.h
index e469d265..93a3bea3 100644
--- a/trunk/fcl/include/fcl/distance_func_matrix.h
+++ b/trunk/fcl/include/fcl/distance_func_matrix.h
@@ -43,12 +43,18 @@
namespace fcl
{
+/// @brief distance matrix stores the functions for distance between different types of objects and provides a uniform call interface
template<typename NarrowPhaseSolver>
struct DistanceFunctionMatrix
{
+ /// @brief the uniform call interface for distance: for distance, we need know
+ /// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2;
+ /// 2. the solver for narrow phase collision, this is for distance computation between geometric shapes;
+ /// 3. the request setting for distance (e.g., whether need to return nearest points);
typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result);
+ /// @brief each item in the distance matrix is a function to handle distance between objects of type1 and type2
DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
DistanceFunctionMatrix();
diff --git a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
index efd171d5..628c1f24 100644
--- a/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
+++ b/trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h
@@ -42,13 +42,12 @@
#include "fcl/BVH_model.h"
#include <boost/math/constants/constants.hpp>
-/** \brief Main namespace */
namespace fcl
{
-/** \brief Generate BVH model from box */
+/// @brief Generate BVH model from box
template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose = Transform3f())
+void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose)
{
double a = shape.side[0];
double b = shape.side[1];
@@ -88,9 +87,9 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f&
model.computeLocalAABB();
}
-/** Generate BVH model from sphere */
+/// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude.
template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose = Transform3f(), unsigned int seg = 16, unsigned int ring = 16)
+void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int seg, unsigned int ring)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
@@ -153,83 +152,24 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3
model.computeLocalAABB();
}
-/** \brief Generate BVH model from sphere
- * The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
- * then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
- */
+/// @brief Generate BVH model from sphere
+/// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
+/// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
template<typename BV>
-void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose = Transform3f(), unsigned int n_faces_for_unit_sphere = 100)
+void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere)
{
- std::vector<Vec3f> points;
- std::vector<Triangle> tri_indices;
-
double r = shape.radius;
-
double n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r;
unsigned int ring = ceil(n_low_bound);
unsigned int seg = ceil(n_low_bound);
- double phi, phid;
- const double pi = boost::math::constants::pi<double>();
- phid = pi * 2 / seg;
- phi = 0;
-
- double theta, thetad;
- thetad = pi / (ring + 1);
- theta = 0;
-
- for(unsigned int i = 0; i < ring; ++i)
- {
- double theta_ = theta + thetad * (i + 1);
- for(unsigned int j = 0; j < seg; ++j)
- {
- points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
- }
- }
- points.push_back(Vec3f(0, 0, r));
- points.push_back(Vec3f(0, 0, -r));
-
- for(unsigned int i = 0; i < ring - 1; ++i)
- {
- for(unsigned int j = 0; j < seg; ++j)
- {
- unsigned int a, b, c, d;
- a = i * seg + j;
- b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
- c = (i + 1) * seg + j;
- d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
- tri_indices.push_back(Triangle(a, c, b));
- tri_indices.push_back(Triangle(b, c, d));
- }
- }
-
- for(unsigned int j = 0; j < seg; ++j)
- {
- unsigned int a, b;
- a = j;
- b = (j == seg - 1) ? 0 : (j + 1);
- tri_indices.push_back(Triangle(ring * seg, a, b));
-
- a = (ring - 1) * seg + j;
- b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
- tri_indices.push_back(Triangle(a, ring * seg + 1, b));
- }
-
- for(unsigned int i = 0; i < points.size(); ++i)
- {
- points[i] = pose.transform(points[i]);
- }
-
- model.beginModel();
- model.addSubModel(points, tri_indices);
- model.endModel();
- model.computeLocalAABB();
+ generateBVHModel(model, shape, pose, seg, ring);
}
-/** \brief Generate BVH model from cylinder */
+/// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose = Transform3f(), unsigned int tot = 16)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
@@ -241,8 +181,6 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transfor
phid = pi * 2 / tot;
phi = 0;
- double circle_edge = phid * r;
- unsigned int h_num = ceil(h / circle_edge);
double hd = h / h_num;
for(unsigned int i = 0; i < tot; ++i)
@@ -301,88 +239,29 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transfor
model.computeLocalAABB();
}
-/** \brief Generate BVH model from cylinder
- * Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
- * larger radius, the number of circle split number is r * tot.
- */
+/// @brief Generate BVH model from cylinder
+/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
+/// larger radius, the number of circle split number is r * tot.
template<typename BV>
-void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose = Transform3f(), unsigned int tot_for_unit_cylinder = 100)
+void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder)
{
- std::vector<Vec3f> points;
- std::vector<Triangle> tri_indices;
-
double r = shape.radius;
double h = shape.lz;
- double phi, phid;
+
const double pi = boost::math::constants::pi<double>();
unsigned int tot = tot_for_unit_cylinder * r;
- phid = pi * 2 / tot;
- phi = 0;
+ double phid = pi * 2 / tot;
double circle_edge = phid * r;
unsigned int h_num = ceil(h / circle_edge);
- double hd = h / h_num;
-
- for(unsigned int i = 0; i < tot; ++i)
- points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
-
- for(unsigned int i = 0; i < h_num - 1; ++i)
- {
- for(unsigned int j = 0; j < tot; ++j)
- {
- points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
- }
- }
-
- for(unsigned int i = 0; i < tot; ++i)
- points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
-
- points.push_back(Vec3f(0, 0, h / 2));
- points.push_back(Vec3f(0, 0, -h / 2));
-
- for(unsigned int i = 0; i < tot; ++i)
- {
- Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
- tri_indices.push_back(tmp);
- }
-
- for(unsigned int i = 0; i < tot; ++i)
- {
- Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
- tri_indices.push_back(tmp);
- }
-
- for(unsigned int i = 0; i < h_num; ++i)
- {
- for(unsigned int j = 0; j < tot; ++j)
- {
- int a, b, c, d;
- a = j;
- b = (j == tot - 1) ? 0 : (j + 1);
- c = j + tot;
- d = (j == tot - 1) ? tot : (j + 1 + tot);
-
- int start = i * tot;
- tri_indices.push_back(Triangle(start + b, start + a, start + c));
- tri_indices.push_back(Triangle(start + b, start + c, start + d));
- }
- }
-
- for(unsigned int i = 0; i < points.size(); ++i)
- {
- points[i] = pose.transform(points[i]);
- }
-
- model.beginModel();
- model.addSubModel(points, tri_indices);
- model.endModel();
- model.computeLocalAABB();
+
+ generateBVHModel(model, shape, pose, tot, h_num);
}
-/** \brief Generate BVH model from cone */
+/// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis.
template<typename BV>
-void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose = Transform3f(), unsigned int tot = 16)
+void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
@@ -395,8 +274,6 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
phid = pi * 2 / tot;
phi = 0;
- double circle_edge = phid * r;
- unsigned int h_num = ceil(h / circle_edge);
double hd = h / h_num;
for(unsigned int i = 0; i < h_num - 1; ++i)
@@ -452,6 +329,25 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
model.computeLocalAABB();
}
+/// @brief Generate BVH model from cone
+/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
+/// larger radius, the number of circle split number is r * tot.
+template<typename BV>
+void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone)
+{
+ double r = shape.radius;
+ double h = shape.lz;
+
+ const double pi = boost::math::constants::pi<double>();
+ unsigned int tot = tot_for_unit_cone * r;
+ double phid = pi * 2 / tot;
+
+ double circle_edge = phid * r;
+ unsigned int h_num = ceil(h / circle_edge);
+
+ generateBVHModel(model, shape, pose, tot, h_num);
+}
+
}
#endif
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index c4288c01..468d3892 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -39,24 +39,23 @@
#define FCL_GEOMETRIC_SHAPES_H
#include "fcl/collision_object.h"
-#include "fcl/vec_3f.h"
+#include "fcl/math/vec_3f.h"
#include <string.h>
-/** \brief Main namespace */
namespace fcl
{
-/** \brief Base class for all basic geometric shapes */
+/// @brief Base class for all basic geometric shapes
class ShapeBase : public CollisionGeometry
{
public:
- /** \brief Default Constructor */
ShapeBase() {}
- /** \brief Get object type: a geometric shape */
+ /// @brief Get object type: a geometric shape
OBJECT_TYPE getObjectType() const { return OT_GEOM; }
};
+/// @brief Triangle stores the points instead of only indices of points
class Triangle2 : public ShapeBase
{
public:
@@ -64,6 +63,7 @@ public:
{
}
+ /// @brief virtual function of compute AABB in local coordinate
void computeLocalAABB();
NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
@@ -71,7 +71,7 @@ public:
Vec3f a, b, c;
};
-/** Center at zero point, axis aligned box */
+/// @brief Center at zero point, axis aligned box
class Box : public ShapeBase
{
public:
@@ -85,17 +85,17 @@ public:
Box() {}
- /** box side length */
+ /// @brief box side length
Vec3f side;
- /** \brief Compute AABB */
+ /// @brief Compute AABB
void computeLocalAABB();
- /** \brief Get node type: a box */
+ /// @brief Get node type: a box
NODE_TYPE getNodeType() const { return GEOM_BOX; }
};
-/** Center at zero point sphere */
+/// @brief Center at zero point sphere
class Sphere : public ShapeBase
{
public:
@@ -103,17 +103,17 @@ public:
{
}
- /** \brief Radius of the sphere */
+ /// @brief Radius of the sphere
FCL_REAL radius;
- /** \brief Compute AABB */
+ /// @brief Compute AABB
void computeLocalAABB();
- /** \brief Get node type: a sphere */
+ /// @brief Get node type: a sphere
NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
};
-/** Center at zero point capsule */
+/// @brief Center at zero point capsule
class Capsule : public ShapeBase
{
public:
@@ -121,20 +121,20 @@ public:
{
}
- /** \brief Radius of capsule */
+ /// @brief Radius of capsule
FCL_REAL radius;
- /** \brief Length along z axis */
+ /// @brief Length along z axis
FCL_REAL lz;
- /** \brief Compute AABB */
+ /// @brief Compute AABB
void computeLocalAABB();
- /** \brief Get node type: a capsule */
+ /// @brief Get node type: a capsule
NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
};
-/** Center at zero cone */
+/// @brief Center at zero cone
class Cone : public ShapeBase
{
public:
@@ -142,21 +142,20 @@ public:
{
}
-
- /** \brief Radius of the cone */
+ /// @brief Radius of the cone
FCL_REAL radius;
- /** \brief Length along z axis */
+ /// @brief Length along z axis
FCL_REAL lz;
- /** \brief Compute AABB */
+ /// @brief Compute AABB
void computeLocalAABB();
- /** \brief Get node type: a cone */
+ /// @brief Get node type: a cone
NODE_TYPE getNodeType() const { return GEOM_CONE; }
};
-/** Center at zero cylinder */
+/// @brief Center at zero cylinder
class Cylinder : public ShapeBase
{
public:
@@ -165,24 +164,24 @@ public:
}
- /** \brief Radius of the cylinder */
+ /// @brief Radius of the cylinder
FCL_REAL radius;
- /** \brief Length along z axis */
+ /// @brief Length along z axis
FCL_REAL lz;
- /** \brief Compute AABB */
+ /// @brief Compute AABB
void computeLocalAABB();
- /** \brief Get node type: a cylinder */
+ /// @brief Get node type: a cylinder
NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
};
-/** Convex polytope */
+/// @brief Convex polytope
class Convex : public ShapeBase
{
public:
- /** Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information */
+ /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information
Convex(Vec3f* plane_normals_,
FCL_REAL* plane_dis_,
int num_planes_,
@@ -208,7 +207,7 @@ public:
fillEdges();
}
- /** Copy constructor */
+ /// @brief Copy constructor
Convex(const Convex& other) : ShapeBase(other)
{
plane_normals = other.plane_normals;
@@ -225,19 +224,18 @@ public:
delete [] edges;
}
- /** Compute AABB */
+ /// @brief Compute AABB
void computeLocalAABB();
- /** Get node type: a conex polytope */
+ /// @brief Get node type: a conex polytope
NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
Vec3f* plane_normals;
FCL_REAL* plane_dis;
- /** An array of indices to the points of each polygon, it should be the number of vertices
- * followed by that amount of indices to "points" in counter clockwise order
- */
+ /// @brief An array of indices to the points of each polygon, it should be the number of vertices
+ /// followed by that amount of indices to "points" in counter clockwise order
int* polygons;
Vec3f* points;
@@ -252,45 +250,45 @@ public:
Edge* edges;
- /** \brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) */
+ /// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex)
Vec3f center;
protected:
- /** \brief Get edge information */
+ /// @brief Get edge information
void fillEdges();
};
-/** Infinite plane */
+/// @brief Infinite plane
class Plane : public ShapeBase
{
public:
- /** \brief Construct a plane with normal direction and offset */
+ /// @brief Construct a plane with normal direction and offset
Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
{
unitNormalTest();
}
- /** \brief Construct a plane with normal direction and offset */
+ /// @brief Construct a plane with normal direction and offset
Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : n(a, b, c), d(d_)
{
unitNormalTest();
}
- /** \brief Compute AABB */
+ /// @brief Compute AABB
void computeLocalAABB();
- /** \brief Get node type: a plane */
+ /// @brief Get node type: a plane
NODE_TYPE getNodeType() const { return GEOM_PLANE; }
- /** \brief Plane normal */
+ /// @brief Plane normal
Vec3f n;
- /** \brief Plane offset */
+ /// @brief Plane offset
FCL_REAL d;
protected:
- /** \brief Turn non-unit normal into unit */
+ /// @brief Turn non-unit normal into unit
void unitNormalTest();
};
diff --git a/trunk/fcl/include/fcl/geometric_shapes_utility.h b/trunk/fcl/include/fcl/geometric_shapes_utility.h
index ffecf921..68cb2869 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_utility.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_utility.h
@@ -45,8 +45,10 @@
namespace fcl
{
+/// @cond IGNORE
namespace details
{
+/// @brief get the vertices of some convex shape which can bound the given shape in a specific configuration
std::vector<Vec3f> getBoundVertices(const Box& box, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Sphere& sphere, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const Transform3f& tf);
@@ -54,9 +56,11 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Convex& convex, const Transform3f& tf);
std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const Transform3f& tf);
-} // end detail
+}
+/// @endcond
+/// @brief calculate a bounding volume for a shape in a specific configuration
template<typename BV, typename S>
void computeBV(const S& s, const Transform3f& tf, BV& bv)
{
@@ -86,7 +90,7 @@ template<>
void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB& bv);
-/** \brief the bounding volume for half space back of plane for OBB, it is the plane itself */
+/// @brief the bounding volume for half space back of plane for OBB, it is the plane itself
template<>
void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv);
@@ -132,6 +136,7 @@ template<>
void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv);
+/// @brief construct a box shape (with a configuration) from a given bounding volume
void constructBox(const AABB& bv, Box& box, Transform3f& tf);
void constructBox(const OBB& bv, Box& box, Transform3f& tf);
diff --git a/trunk/fcl/include/fcl/input_represent.h b/trunk/fcl/include/fcl/input_represent.h
deleted file mode 100644
index b168d6f0..00000000
--- a/trunk/fcl/include/fcl/input_represent.h
+++ /dev/null
@@ -1,178 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * 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 Willow Garage, Inc. 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 Jia Pan */
-
-#ifndef FCL_INPUT_REPRESENT
-#define FCL_INPUT_REPRESENT
-
-#include "fcl/data_types.h"
-#include "fcl/vec_3f.h"
-
-
-namespace fcl
-{
-
-
-struct PointFunctor
-{
- virtual void operator() (const Vec3f& v);
-};
-
-struct VarianceStatistics : public PointFunctor
-{
- VarianceStatistics()
- {
- for(unsigned int i = 0; i < 3; ++i)
- S[i] = Vec3f();
- n = 0;
- }
-
- void operator() (const Vec3f& v)
- {
- m += v;
- S[0][0] += v[0] * v[0];
- S[0][1] += v[0] * v[1];
- S[0][2] += v[0] * v[2];
- S[1][1] += v[1] * v[1];
- S[1][2] += v[1] * v[2];
- S[2][2] += v[2] * v[2];
- n++;
- }
-
- void reduce(Vec3f& mean, Matrix3f& var)
- {
- mean = m;
- var[0][0] = S[0][0] - m[0] * m[0] / n;
- var[1][1] = S[1][1] - m[1] * m[1] / n;
- var[2][2] = S[2][2] - m[2] * m[2] / n;
- var[0][1] = S[0][1] - m[0] * m[1] / n;
- var[0][2] = S[0][2] - m[0] * m[2] / n;
- var[1][2] = S[1][2] - m[1] * m[2] / n;
- }
-
- Vec3f m;
- Vec3f S[3];
- unsigned int n;
-};
-
-class Mesh
-{
-public:
- typedef PrimitiveType Triangle;
-
- Vec3f* vertices;
- Vec3f* prev_vertices;
- Triangle* triangles;
-
- unsigned int n_vertices;
- unsigned int n_tris;
-
- Mesh(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* triangles_)
- {
- vertices = vertices_;
- prev_vertices = prev_vertices_;
- triangles = triangles_;
- }
-
- void operate(PointFunctor& func)
- {
- for(unsigned int i = 0; i < n_tris; ++i)
- {
- const Triangle& t = *(triangles + i);
-
- for(unsigned int j = 0; j < 3; ++j)
- {
- unsigned int point_id = t[j];
- func(vertices[point_id]);
- if(prev_vertices) func(prev_vertices[point_id]);
- }
- }
- }
-
- void operate(PointFunctor& func, unsigned int* indices, unsigned int n)
- {
- for(unsigned int i = 0; i < n; ++i)
- {
- const Triangle& t = *(triangles + indices[i]);
- for(unsigned int j = 0; j < 3; ++j)
- {
- unsigned int point_id = t[j];
- func(vertices[point_id]);
- if(prev_vertices) func(prev_vertices[point_id]);
- }
- }
- }
-};
-
-class PointCloud
-{
-public:
- typedef PrimitiveType Vec3f;
-
- Vec3f* vertices;
- Vec3f* prev_vertices;
-
- unsigned int n_vertices;
-
- PointCloud(Vec3f* vertices_, Vec3f* prev_vertices_)
- {
- vertices = vertices_;
- prev_vertices = prev_vertices_;
- }
-
- void operate(PointFunctor& func)
- {
- for(unsigned int i = 0; i < n_vertices; ++i)
- {
- func(vertices[i]);
- if(prev_vertices) func(prev_vertices[i]);
- }
- }
-
- void operate(PointFunctor& func, unsigned int* indices, unsigned int n)
- {
- for(unsigned int i = 0; i < n; ++i)
- {
- unsigned int point_id = indices[i];
- func(vertices[point_id]);
- if(prev_vertices) func(prev_vertices[point_id]);
- }
- }
-};
-
-
-}
-
-#endif
diff --git a/trunk/fcl/include/fcl/intersect.h b/trunk/fcl/include/fcl/intersect.h
index 551f5f79..16a1ec34 100644
--- a/trunk/fcl/include/fcl/intersect.h
+++ b/trunk/fcl/include/fcl/intersect.h
@@ -37,97 +37,76 @@
#ifndef FCL_INTERSECT_H
#define FCL_INTERSECT_H
-#include "fcl/transform.h"
+#include "fcl/math/transform.h"
#include "fcl/BVH_internal.h"
#include "fcl/data_types.h"
-#if USE_SVMLIGHT
-extern "C"
-{
-#include <svm_light/svm_common.h>
-#include <svm_light/svm_learn.h>
-}
-#endif
-/** \brief Main namespace */
namespace fcl
{
-/** \brief A class solves polynomial degree (1,2,3) equations */
+/// @brief A class solves polynomial degree (1,2,3) equations
class PolySolver
{
public:
- /** \brief Solve a linear equation with coefficients c, return roots s and number of roots */
+ /// @brief Solve a linear equation with coefficients c, return roots s and number of roots
static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]);
- /** \brief Solve a quadratic function with coefficients c, return roots s and number of roots */
+ /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots
static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]);
- /** \brief Solve a cubic function with coefficients c, return roots s and number of roots */
+ /// @brief Solve a cubic function with coefficients c, return roots s and number of roots
static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]);
private:
- /** \brief Check whether v is zero */
+ /// @brief Check whether v is zero
static inline bool isZero(FCL_REAL v);
- /** \brief Compute v^{1/3} */
+ /// @brief Compute v^{1/3}
static inline bool cbrt(FCL_REAL v);
static const FCL_REAL NEAR_ZERO_THRESHOLD;
};
-#if USE_SVMLIGHT
-class CloudClassifierParam
-{
-public:
- LEARN_PARM learn_parm;
- KERNEL_PARM kernel_parm;
-
- CloudClassifierParam();
-};
-#endif
-
-/** \brief CCD intersect kernel among primitives */
+/// @brief CCD intersect kernel among primitives
class Intersect
{
public:
- /** \brief CCD intersect between one vertex and one face
- * [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1
- * p0 and p1 are points for vertex in time t0 and t1
- * p_i returns the coordinate of the collision point
- */
+ /// @brief CCD intersect between one vertex and one face
+ /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1
+ /// p0 and p1 are points for vertex in time t0 and t1
+ /// p_i returns the coordinate of the collision point
static bool intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
- /** \brief CCD intersect between two edges
- * [a0, b0] and [a1, b1] are points for one edge in time t0 and t1
- * [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1
- * p_i returns the coordinate of the collision point
- */
+ /// @brief CCD intersect between two edges
+ /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1
+ /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1
+ /// p_i returns the coordinate of the collision point
static bool intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
- /** \brief CCD intersect between one vertex and one face, using additional filter */
+ /// @brief CCD intersect between one vertex and one face, using additional filter
static bool intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
- /** \brief CCD intersect between two edges, using additional filter */
+ /// @brief CCD intersect between two edges, using additional filter
static bool intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
FCL_REAL* collision_time, Vec3f* p_i, bool useNewton = true);
- /** \brief CCD intersect between one vertex and and one edge */
+ /// @brief CCD intersect between one vertex and and one edge
static bool intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
const Vec3f& L);
- /** \brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] */
+ /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3]
static bool intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
Vec3f* contact_points = NULL,
@@ -151,144 +130,117 @@ public:
FCL_REAL* penetration_depth = NULL,
Vec3f* normal = NULL);
-
-#if USE_SVMLIGHT
-
- static FCL_REAL intersect_PointClouds(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
- Vec3f* cloud2, Variance3f* uc2, int size_cloud2,
- const CloudClassifierParam& solver, bool scaling = true);
-
- static FCL_REAL intersect_PointClouds(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
- Vec3f* cloud2, Variance3f* uc2, int size_cloud2,
- const Matrix3f& R, const Vec3f& T, const CloudClassifierParam& solver, bool scaling = true);
-
- static FCL_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3);
-
- static FCL_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Matrix3f& R, const Vec3f& T);
-#endif
-
private:
- /** \brief Project function used in intersect_Triangle() */
+ /// @brief Project function used in intersect_Triangle()
static int project6(const Vec3f& ax,
const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
const Vec3f& q1, const Vec3f& q2, const Vec3f& q3);
- /** \brief Check whether one value is zero */
+ /// @brief Check whether one value is zero
static inline bool isZero(FCL_REAL v);
- /** \brief Solve the cubic function using Newton method, also satisfies the interval restriction */
+ /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction
static bool solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data = NULL);
- /** \brief Check whether one point p is within triangle [a, b, c] */
+ /// @brief Check whether one point p is within triangle [a, b, c]
static bool insideTriangle(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f&p);
- /** \brief Check whether one point p is within a line segment [a, b] */
+ /// @brief Check whether one point p is within a line segment [a, b]
static bool insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p);
- /** \brief Calculate the line segment papb that is the shortest route between
- * two lines p1p2 and p3p4. Calculate also the values of mua and mub where
- * pa = p1 + mua (p2 - p1)
- * pb = p3 + mub (p4 - p3)
- * return FALSE if no solution exists.
- */
+ /// @brief Calculate the line segment papb that is the shortest route between
+ /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where
+ /// pa = p1 + mua (p2 - p1)
+ /// pb = p3 + mub (p4 - p3)
+ /// return FALSE if no solution exists.
static bool linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub);
- /** \brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t */
+ /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t
static bool checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
FCL_REAL t);
- /** \brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time */
+ /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time
static bool checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
FCL_REAL t, Vec3f* q_i = NULL);
- /** \brief Check whether a root for VE intersection is valid */
+ /// @brief Check whether a root for VE intersection is valid
static bool checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
FCL_REAL t);
- /** \brief Solve a square function for EE intersection (with interval restriction) */
+ /// @brief Solve a square function for EE intersection (with interval restriction)
static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
bool bVF,
FCL_REAL* ret);
- /** \brief Solve a square function for VE intersection (with interval restriction) */
+ /// @brief Solve a square function for VE intersection (with interval restriction)
static bool solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp);
- /** \brief Compute the cubic coefficients for VF intersection
- * See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
- */
+ /// @brief Compute the cubic coefficients for VF intersection
+ /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
+
static void computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
- /** \brief Compute the cubic coefficients for EE intersection */
+ /// @brief Compute the cubic coefficients for EE intersection
static void computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d);
- /** \brief Compute the cubic coefficients for VE intersection */
+ /// @brief Compute the cubic coefficients for VE intersection
static void computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
const Vec3f& L,
FCL_REAL* a, FCL_REAL* b, FCL_REAL* c);
- /** \brief filter for intersection, works for both VF and EE */
+ /// @brief filter for intersection, works for both VF and EE
static bool intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1);
- /** \brief distance of point v to a plane n * x - t = 0 */
+ /// @brief distance of point v to a plane n * x - t = 0
static FCL_REAL distanceToPlane(const Vec3f& n, FCL_REAL t, const Vec3f& v);
- /** \brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 */
+ /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0
static bool sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, const Vec3f& n, FCL_REAL t);
- /** \brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to */
+ /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to
static void clipTriangleByTriangleAndEdgePlanes(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3,
const Vec3f& t1, const Vec3f& t2, const Vec3f& t3,
const Vec3f& tn, FCL_REAL to,
Vec3f clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false);
- /** \brief build a plane passed through triangle v1 v2 v3 */
+ /// @brief build a plane passed through triangle v1 v2 v3
static bool buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
- /** \brief build a plane pass through edge v1 and v2, normal is tn */
+ /// @brief build a plane pass through edge v1 and v2, normal is tn
static bool buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn, Vec3f* n, FCL_REAL* t);
- /** \brief compute the points which has deepest penetration depth */
+ /// @brief compute the points which has deepest penetration depth
static void computeDeepestPoints(Vec3f* clipped_points, unsigned int num_clipped_points, const Vec3f& n, FCL_REAL t, FCL_REAL* penetration_depth, Vec3f* deepest_points, unsigned int* num_deepest_points);
- /** \brief clip polygon by plane */
+ /// @brief clip polygon by plane
static void clipPolygonByPlane(Vec3f* polygon_points, unsigned int num_polygon_points, const Vec3f& n, FCL_REAL t, Vec3f clipped_points[], unsigned int* num_clipped_points);
- /** \brief clip a line segment by plane */
+ /// @brief clip a line segment by plane
static void clipSegmentByPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& n, FCL_REAL t, Vec3f* clipped_point);
- /** \brief compute the cdf(x) */
+ /// @brief compute the cdf(x)
static FCL_REAL gaussianCDF(FCL_REAL x)
{
return 0.5 * erfc(-x / sqrt(2.0));
}
-#if USE_SVMLIGHT
- /** \brief compute the d K(x0, x) / dx, where x is one 3d point on or near the classification surface, and K is a composite kernel */
- static void kernelGradient(KERNEL_PARM *kernel_parm, DOC *a, DOC *b, Vec3f& g);
-
- /** \brief compute the d K(x0, x) / dx, where x is one 3d point on or near the classification surface, and K is a single kernel */
- static void singleKernelGradient(KERNEL_PARM *kernel_parm, SVECTOR *a, SVECTOR *b, Vec3f& g);
-#endif
static const FCL_REAL EPSILON;
static const FCL_REAL NEAR_ZERO_THRESHOLD;
@@ -300,34 +252,31 @@ class TriangleDistance
{
public:
- /** \brief Returns closest points between an segment pair.
- * The first segment is P + t * A
- * The second segment is Q + t * B
- * X, Y are the closest points on the two segments
- * VEC is the vector between X and Y
- */
+ /// @brief Returns closest points between an segment pair.
+ /// The first segment is P + t * A
+ /// The second segment is Q + t * B
+ /// X, Y are the closest points on the two segments
+ /// VEC is the vector between X and Y
static void segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
Vec3f& VEC, Vec3f& X, Vec3f& Y);
- /** \brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them
- * S and T are two triangles
- * If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
- * if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
- * coincident points on the intersection of the triangles, as might be expected.
- */
+ /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them
+ /// S and T are two triangles
+ /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
+ /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
+ /// coincident points on the intersection of the triangles, as might be expected.
static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3], Vec3f& P, Vec3f& Q);
static FCL_REAL triDistance(const Vec3f& S1, const Vec3f& S2, const Vec3f& S3,
const Vec3f& T1, const Vec3f& T2, const Vec3f& T3,
Vec3f& P, Vec3f& Q);
- /** \brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them
- * S and T are two triangles
- * If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
- * if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
- * coincident points on the intersection of the triangles, as might be expected.
- * The returned P and Q are both in the coordinate of the first triangle's coordinate
- */
+ /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them
+ /// S and T are two triangles
+ /// If the triangles are disjoint, P and Q give the closet points of S and T respectively. However,
+ /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not
+ /// coincident points on the intersection of the triangles, as might be expected.
+ /// The returned P and Q are both in the coordinate of the first triangle's coordinate
static FCL_REAL triDistance(const Vec3f S[3], const Vec3f T[3],
const Matrix3f& R, const Vec3f& Tl,
Vec3f& P, Vec3f& Q);
diff --git a/trunk/fcl/include/fcl/math_details.h b/trunk/fcl/include/fcl/math/math_details.h
similarity index 100%
rename from trunk/fcl/include/fcl/math_details.h
rename to trunk/fcl/include/fcl/math/math_details.h
diff --git a/trunk/fcl/include/fcl/matrix_3f.h b/trunk/fcl/include/fcl/math/matrix_3f.h
similarity index 97%
rename from trunk/fcl/include/fcl/matrix_3f.h
rename to trunk/fcl/include/fcl/math/matrix_3f.h
index 5eb4754b..36fc4352 100644
--- a/trunk/fcl/include/fcl/matrix_3f.h
+++ b/trunk/fcl/include/fcl/math/matrix_3f.h
@@ -37,12 +37,12 @@
#ifndef FCL_MATRIX_3F_H
#define FCL_MATRIX_3F_H
-#include "fcl/vec_3f.h"
+#include "fcl/math/vec_3f.h"
namespace fcl
{
-
+/// @brief Matrix2 class wrapper. the core data is in the template parameter class
template<typename T>
class Matrix3fX
{
@@ -293,6 +293,7 @@ void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_t
t = R1.transposeTimes(t2 - t1);
}
+/// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
template<typename T>
void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
{
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/math/transform.h
similarity index 63%
rename from trunk/fcl/include/fcl/transform.h
rename to trunk/fcl/include/fcl/math/transform.h
index 979885e3..461f4d49 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/math/transform.h
@@ -38,17 +38,17 @@
#ifndef FCL_TRANSFORM_H
#define FCL_TRANSFORM_H
-#include "fcl/vec_3f.h"
-#include "fcl/matrix_3f.h"
+#include "fcl/math/matrix_3f.h"
+#include <boost/thread/mutex.hpp>
namespace fcl
{
-/** \brief Quaternion used locally by InterpMotion */
+/// @brief Quaternion used locally by InterpMotion
class Quaternion3f
{
public:
- /** \brief Default quaternion is identity rotation */
+ /// @brief Default quaternion is identity rotation
Quaternion3f()
{
data[0] = 1;
@@ -65,58 +65,59 @@ public:
data[3] = d;
}
+ /// @brief Whether the rotation is identity
bool isIdentity() const
{
return (data[0] == 1) && (data[1] == 0) && (data[2] == 0) && (data[3] == 0);
}
- /** \brief Matrix to quaternion */
+ /// @brief Matrix to quaternion
void fromRotation(const Matrix3f& R);
- /** \brief Quaternion to matrix */
+ /// @brief Quaternion to matrix
void toRotation(Matrix3f& R) const;
- /** \brief Axes to quaternion */
+ /// @brief Axes to quaternion
void fromAxes(const Vec3f axis[3]);
- /** \brief Axes to matrix */
+ /// @brief Axes to matrix
void toAxes(Vec3f axis[3]) const;
- /** \brief Axis and angle to quaternion */
+ /// @brief Axis and angle to quaternion
void fromAxisAngle(const Vec3f& axis, FCL_REAL angle);
- /** \brief Quaternion to axis and angle */
+ /// @brief Quaternion to axis and angle
void toAxisAngle(Vec3f& axis, FCL_REAL& angle) const;
- /** \brief Dot product between quaternions */
+ /// @brief Dot product between quaternions
FCL_REAL dot(const Quaternion3f& other) const;
- /** \brief addition */
+ /// @brief addition
Quaternion3f operator + (const Quaternion3f& other) const;
const Quaternion3f& operator += (const Quaternion3f& other);
- /** \brief minus */
+ /// @brief minus
Quaternion3f operator - (const Quaternion3f& other) const;
const Quaternion3f& operator -= (const Quaternion3f& other);
- /** \brief multiplication */
+ /// @brief multiplication
Quaternion3f operator * (const Quaternion3f& other) const;
const Quaternion3f& operator *= (const Quaternion3f& other);
- /** \brief division */
+ /// @brief division
Quaternion3f operator - () const;
- /** \brief scalar multiplication */
+ /// @brief scalar multiplication
Quaternion3f operator * (FCL_REAL t) const;
const Quaternion3f& operator *= (FCL_REAL t);
- /** \brief conjugate */
+ /// @brief conjugate
Quaternion3f& conj();
- /** \brief inverse */
+ /// @brief inverse
Quaternion3f& inverse();
- /** \brief rotate a vector */
+ /// @brief rotate a vector
Vec3f transform(const Vec3f& v) const;
inline const FCL_REAL& getW() const { return data[0]; }
@@ -134,92 +135,119 @@ private:
FCL_REAL data[4];
};
+/// @brief conjugate of quaternion
Quaternion3f conj(const Quaternion3f& q);
+
+/// @brief inverse of quaternion
Quaternion3f inverse(const Quaternion3f& q);
-/** \brief Simple transform class used locally by InterpMotion */
+/// @brief Simple transform class used locally by InterpMotion
class Transform3f
{
- /** \brief Whether matrix cache is set */
+ boost::mutex lock_;
+
+ /// @brief Whether matrix cache is set
mutable bool matrix_set;
- /** \brief Matrix cache */
+ /// @brief Matrix cache
mutable Matrix3f R;
-
- /** \brief Tranlation vector */
+ /// @brief Tranlation vector
Vec3f T;
- /** \brief Rotation*/
+ /// @brief Rotation
Quaternion3f q;
+ const Matrix3f& getRotationInternal() const;
public:
- /** \brief Default transform is no movement */
+ /// @brief Default transform is no movement
Transform3f()
{
setIdentity(); // set matrix_set true
}
+ /// @brief Construct transform from rotation and translation
Transform3f(const Matrix3f& R_, const Vec3f& T_) : matrix_set(true),
- R(R_),
- T(T_)
+ R(R_),
+ T(T_)
{
q.fromRotation(R_);
}
+ /// @brief Construct transform from rotation and translation
Transform3f(const Quaternion3f& q_, const Vec3f& T_) : matrix_set(false),
- T(T_),
- q(q_)
+ T(T_),
+ q(q_)
{
}
+ /// @brief Construct transform from rotation
Transform3f(const Matrix3f& R_) : matrix_set(true),
- R(R_)
+ R(R_)
{
q.fromRotation(R_);
}
+ /// @brief Construct transform from rotation
Transform3f(const Quaternion3f& q_) : matrix_set(false),
- q(q_)
+ q(q_)
{
}
+ /// @brief Construct transform from translation
Transform3f(const Vec3f& T_) : matrix_set(true),
- T(T_)
+ T(T_)
{
R.setIdentity();
}
+ /// @brief Construct transform from another transform
+ Transform3f(const Transform3f& tf) : matrix_set(tf.matrix_set),
+ R(tf.R),
+ T(tf.T),
+ q(tf.q)
+ {
+ }
+
+ /// @brief operator =
+ Transform3f& operator = (const Transform3f& tf)
+ {
+ matrix_set = tf.matrix_set;
+ R = tf.R;
+ q = tf.q;
+ T = tf.T;
+ return *this;
+ }
+
+ /// @brief get translation
inline const Vec3f& getTranslation() const
{
return T;
}
+ /// @brief get rotation
inline const Matrix3f& getRotation() const
{
- if(!matrix_set)
- {
- q.toRotation(R);
- matrix_set = true;
- }
-
- return R;
+ if(matrix_set) return R;
+ return getRotationInternal();
}
+ /// @brief get quaternion
inline const Quaternion3f& getQuatRotation() const
{
return q;
}
+ /// @brief set transform from rotation and translation
inline void setTransform(const Matrix3f& R_, const Vec3f& T_)
{
- matrix_set = true;
R = R_;
T = T_;
-
+ matrix_set = true;
q.fromRotation(R_);
}
+ /// @brief set transform from rotation and translation
inline void setTransform(const Quaternion3f& q_, const Vec3f& T_)
{
matrix_set = false;
@@ -227,30 +255,35 @@ public:
T = T_;
}
+ /// @brief set transform from rotation
inline void setRotation(const Matrix3f& R_)
{
- matrix_set = true;
R = R_;
+ matrix_set = true;
q.fromRotation(R_);
}
+ /// @brief set transform from translation
inline void setTranslation(const Vec3f& T_)
{
T = T_;
}
+ /// @brief set transform from rotation
inline void setQuatRotation(const Quaternion3f& q_)
{
matrix_set = false;
q = q_;
}
- Vec3f transform(const Vec3f& v) const
+ /// @brief transform a given vector by the transform
+ inline Vec3f transform(const Vec3f& v) const
{
return q.transform(v) + T;
}
- Transform3f& inverse()
+ /// @brief inverse transform
+ inline Transform3f& inverse()
{
matrix_set = false;
q.conj();
@@ -258,13 +291,15 @@ public:
return *this;
}
- Transform3f inverseTimes(const Transform3f& other) const
+ /// @brief inverse the transform and multiply with another
+ inline Transform3f inverseTimes(const Transform3f& other) const
{
const Quaternion3f& q_inv = fcl::conj(q);
return Transform3f(q_inv * other.q, q_inv.transform(other.T - T));
}
- const Transform3f& operator *= (const Transform3f& other)
+ /// @brief multiply with another transform
+ inline const Transform3f& operator *= (const Transform3f& other)
{
matrix_set = false;
T = q.transform(other.T) + T;
@@ -272,18 +307,21 @@ public:
return *this;
}
- Transform3f operator * (const Transform3f& other) const
+ /// @brief multiply with another transform
+ inline Transform3f operator * (const Transform3f& other) const
{
Quaternion3f q_new = q * other.q;
return Transform3f(q_new, q.transform(other.T) + T);
}
- bool isIdentity() const
+ /// @brief check whether the transform is identity
+ inline bool isIdentity() const
{
return q.isIdentity() && T.isZero();
}
- void setIdentity()
+ /// @brief set the transform to be identity transform
+ inline void setIdentity()
{
R.setIdentity();
T.setValue(0);
@@ -293,8 +331,10 @@ public:
};
+/// @brief inverse the transform
Transform3f inverse(const Transform3f& tf);
+/// @brief compute the relative transform between two transforms: tf2 = tf * tf1
void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf);
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/math/vec_3f.h
similarity index 95%
rename from trunk/fcl/include/fcl/vec_3f.h
rename to trunk/fcl/include/fcl/math/vec_3f.h
index 9573fe99..fb6e6721 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/math/vec_3f.h
@@ -38,8 +38,8 @@
#define FCL_VEC_3F_H
#include "fcl/BVH_internal.h"
-#include "fcl/math_details.h"
-#include "fcl/math_simd_details.h"
+#include "fcl/math/math_details.h"
+#include "fcl/simd/math_simd_details.h"
#include <cmath>
#include <iostream>
#include <limits>
@@ -48,17 +48,26 @@
namespace fcl
{
+/// @brief Vector3 class wrapper. The core data is in the template parameter class.
template <typename T>
class Vec3fX
{
public:
typedef typename T::meta_type U;
+
+ /// @brief interval vector3 data
T data;
Vec3fX() {}
Vec3fX(const Vec3fX& other) : data(other.data) {}
+
+ /// @brief create Vector (x, y, z)
Vec3fX(U x, U y, U z) : data(x, y, z) {}
+
+ /// @brief create vector (x, x, x)
Vec3fX(U x) : data(x) {}
+
+ /// @brief create vector using the internal data type
Vec3fX(const T& data_) : data(data_) {}
inline U operator [] (size_t i) const { return data[i]; }
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk.h b/trunk/fcl/include/fcl/narrowphase/gjk.h
index f29a9351..1943851f 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk.h
@@ -38,8 +38,7 @@
#define FCL_GJK_H
#include "fcl/geometric_shapes.h"
-#include "fcl/matrix_3f.h"
-#include "fcl/transform.h"
+#include "fcl/math/transform.h"
namespace fcl
{
@@ -47,21 +46,30 @@ namespace fcl
namespace details
{
+/// @brief the support function for shape
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir);
+/// @brief Minkowski difference class of two shapes
struct MinkowskiDiff
{
+ /// @brief points to two shapes
const ShapeBase* shapes[2];
+
+ /// @brief rotation from shape0 to shape1
Matrix3f toshape1;
+
+ /// @brief transform from shape1 to shape0
Transform3f toshape0;
MinkowskiDiff() { }
+ /// @brief support function for shape0
inline Vec3f support0(const Vec3f& d) const
{
return getSupport(shapes[0], d);
}
+ /// @brief support function for shape1
inline Vec3f support1(const Vec3f& d) const
{
return toshape0.transform(getSupport(shapes[1], toshape1 * d));
@@ -82,26 +90,41 @@ struct MinkowskiDiff
};
-
+/// @brief project origin on to a line (a, b). w[0:1] return the (0, 1) parameterization of the projected point.
+/// m is a encode about the result case: 0x10--> project is larger than b; 0x01--> project is smaller than a;
+/// 0x11-> within the line;
+/// return value is distance between the origin and its projection.
FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m);
+/// @brief project origin on to a triangle (a, b, c). w[0:2] return the (0, 1) parameterization of the projected point.
+/// m is a encode about the result case.
+/// return value is distance between the origin and its projection.
FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m);
+/// @brief project origin on to a tetrahedra (a, b, c, d). w[0:3] return the (0, 1) parameterization of the projected point.
+/// m is a encode about the result case.
+/// return value is distance between the origin and its projection.
FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m);
+/// @brief class for GJK algorithm
struct GJK
{
struct SimplexV
{
- Vec3f d; // support direction
- Vec3f w; // support vector
+ /// @brief support direction
+ Vec3f d;
+ /// @brieg support vector
+ Vec3f w;
};
struct Simplex
{
- SimplexV* c[4]; // simplex vertex
- FCL_REAL p[4]; // weight
- size_t rank; // size of simplex (number of vertices)
+ /// @brief simplex vertex
+ SimplexV* c[4];
+ /// @brief weight
+ FCL_REAL p[4];
+ /// @brief size of simplex (number of vertices)
+ size_t rank;
};
enum Status {Valid, Inside, Failed};
@@ -112,26 +135,30 @@ struct GJK
Simplex simplices[2];
- GJK(unsigned int max_iterations_, FCL_REAL tolerance_)
+ GJK(unsigned int max_iterations_, FCL_REAL tolerance_) : max_iterations(max_iterations_),
+ tolerance(tolerance_)
{
- max_iterations = max_iterations_;
- tolerance = tolerance_;
-
initialize();
}
void initialize();
+ /// @brief GJK algorithm, given the initial value guess
Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess);
+ /// @brief apply the support function along a direction, the result is return in sv
void getSupport(const Vec3f& d, SimplexV& sv) const;
+ /// @brief discard one vertex from the simplex
void removeVertex(Simplex& simplex);
+ /// @brief append one vertex to the simplex
void appendVertex(Simplex& simplex, const Vec3f& v);
+ /// @brief whether the simplex enclose the origin
bool encloseOrigin();
+ /// @brief get the underlying simplex using in GJK
inline Simplex* getSimplex() const
{
return simplex;
@@ -145,8 +172,9 @@ private:
Simplex* simplex;
Status status;
- FCL_REAL tolerance;
unsigned int max_iterations;
+ FCL_REAL tolerance;
+
};
@@ -155,6 +183,7 @@ static const size_t EPA_MAX_VERTICES = 64;
static const FCL_REAL EPA_EPS = 0.000001;
static const size_t EPA_MAX_ITERATIONS = 255;
+/// @brief class for EPA algorithm
struct EPA
{
private:
@@ -226,13 +255,11 @@ public:
size_t nextsv;
SimplexList hull, stock;
- EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_)
+ EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, FCL_REAL tolerance_) : max_face_num(max_face_num_),
+ max_vertex_num(max_vertex_num_),
+ max_iterations(max_iterations_),
+ tolerance(tolerance_)
{
- max_face_num = max_face_num_;
- max_vertex_num = max_vertex_num_;
- max_iterations = max_iterations_;
- tolerance = tolerance_;
-
initialize();
}
@@ -248,12 +275,12 @@ public:
SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced);
- /** \brief Find the best polytope face to split */
+ /// @brief Find the best polytope face to split
SimplexF* findBest();
Status evaluate(GJK& gjk, const Vec3f& guess);
- /** \brief the goal is to add a face connecting vertex w and face edge f[e] */
+ /// @brief the goal is to add a face connecting vertex w and face edge f[e]
bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);
};
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
index 4c110661..985d8d6d 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
@@ -39,7 +39,7 @@
#define FCL_GJK_LIBCCD_H
#include "fcl/geometric_shapes.h"
-#include "fcl/transform.h"
+#include "fcl/math/transform.h"
#include <ccd/ccd.h>
#include <ccd/quat.h>
@@ -50,32 +50,31 @@ namespace fcl
namespace details
{
-/** \brief recall function used by GJK algorithm */
+/// @brief callback function used by GJK algorithm
typedef void (*GJKSupportFunction)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v);
typedef void (*GJKCenterFunction)(const void* obj, ccd_vec3_t* c);
-/** \brief initialize GJK stuffs */
+/// @brief initialize GJK stuffs
template<typename T>
class GJKInitializer
{
public:
- /** \brief Get GJK support function */
+ /// @brief Get GJK support function
static GJKSupportFunction getSupportFunction() { return NULL; }
- /** \brief Get GJK center function */
+ /// @brief Get GJK center function
static GJKCenterFunction getCenterFunction() { return NULL; }
- /** \brief Get GJK object from a shape
- * Notice that only local transformation is applied.
- * Gloal transformation are considered later
- */
+ /// @brief Get GJK object from a shape
+ /// Notice that only local transformation is applied.
+ /// Gloal transformation are considered later
static void* createGJKObject(const T& s, const Transform3f& tf) { return NULL; }
- /** \brief Delete GJK object */
+ /// @brief Delete GJK object
static void deleteGJKObject(void* o) {}
};
-/** \brief initialize GJK Cylinder */
+/// @brief initialize GJK Cylinder
template<>
class GJKInitializer<Cylinder>
{
@@ -86,7 +85,7 @@ public:
static void deleteGJKObject(void* o);
};
-/** \brief initialize GJK Sphere */
+/// @brief initialize GJK Sphere
template<>
class GJKInitializer<Sphere>
{
@@ -97,7 +96,7 @@ public:
static void deleteGJKObject(void* o);
};
-/** \brief initialize GJK Box */
+/// @brief initialize GJK Box
template<>
class GJKInitializer<Box>
{
@@ -108,7 +107,7 @@ public:
static void deleteGJKObject(void* o);
};
-/** \brief initialize GJK Capsule */
+/// @brief initialize GJK Capsule
template<>
class GJKInitializer<Capsule>
{
@@ -119,7 +118,7 @@ public:
static void deleteGJKObject(void* o);
};
-/** \brief initialize GJK Cone */
+/// @brief initialize GJK Cone
template<>
class GJKInitializer<Cone>
{
@@ -130,7 +129,7 @@ public:
static void deleteGJKObject(void* o);
};
-/** \brief initialize GJK Convex */
+/// @brief initialize GJK Convex
template<>
class GJKInitializer<Convex>
{
@@ -141,7 +140,7 @@ public:
static void deleteGJKObject(void* o);
};
-/** \brief initialize GJK Triangle */
+/// @brief initialize GJK Triangle
GJKSupportFunction triGetSupportFunction();
GJKCenterFunction triGetCenterFunction();
@@ -152,7 +151,7 @@ void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, cons
void triDeleteGJKObject(void* o);
-/** \brief GJK collision algorithm */
+/// @brief GJK collision algorithm
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
unsigned int max_iterations, FCL_REAL tolerance,
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
index adcc3d1b..f870aaa5 100644
--- a/trunk/fcl/include/fcl/narrowphase/narrowphase.h
+++ b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
@@ -45,9 +45,10 @@
namespace fcl
{
+/// @brief collision and distance solver based on libccd library.
struct GJKSolver_libccd
{
- /** intersection checking between two shapes */
+ /// @brief intersection checking between two shapes
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
@@ -67,7 +68,7 @@ struct GJKSolver_libccd
return res;
}
- /** \brief intersection checking between one shape and a triangle */
+ /// @brief intersection checking between one shape and a triangle
template<typename S>
bool shapeTriangleIntersect(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
@@ -86,7 +87,7 @@ struct GJKSolver_libccd
return res;
}
- /** \brief intersection checking between one shape and a triangle with transformation */
+ //// @brief intersection checking between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
@@ -107,7 +108,7 @@ struct GJKSolver_libccd
}
- /** \brief distance computation between two shapes */
+ /// @brief distance computation between two shapes
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
@@ -128,7 +129,7 @@ struct GJKSolver_libccd
}
- /** \brief distance computation between one shape and a triangle */
+ /// @brief distance computation between one shape and a triangle
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
@@ -148,7 +149,7 @@ struct GJKSolver_libccd
return res;
}
- /** \brief distance computation between one shape and a triangle with transformation */
+ /// @brief distance computation between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
@@ -169,6 +170,7 @@ struct GJKSolver_libccd
}
+ /// @brief default setting for GJK algorithm
GJKSolver_libccd()
{
max_collision_iterations = 500;
@@ -177,58 +179,67 @@ struct GJKSolver_libccd
distance_tolerance = 1e-6;
}
+ /// @brief maximum number of iterations used in GJK algorithm for collision
unsigned int max_collision_iterations;
+
+ /// @brief maximum number of iterations used in GJK algorithm for distance
unsigned int max_distance_iterations;
+
+ /// @brief the threshold used in GJK algorithm to stop collision iteration
FCL_REAL collision_tolerance;
+
+ /// @brief the threshold used in GJK algorithm to stop distance iteration
FCL_REAL distance_tolerance;
};
-/** \brief Fast implementation for sphere-sphere collision */
+/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/** \brief Fast implementation for sphere-triangle collision */
+/// @brief Fast implementation for sphere-triangle collision
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/** \brief Fast implementation for sphere-triangle collision */
+/// @brief Fast implementation for sphere-triangle collision
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/** \brief Fast implementation for sphere-sphere distance */
+/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist) const;
-/** \brief Fast implementation for sphere-triangle distance */
+/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist) const;
-/** \brief Fast implementation for sphere-triangle distance */
+/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_libccd::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist) const;
-/** \brief Fast implementation for box-box collision */
+/// @brief Fast implementation for box-box collision
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
+/// @brief collision and distance solver based on GJK algorithm implemented in fcl (rewritten the code from the GJK in bullet)
struct GJKSolver_indep
{
+ /// @brief intersection checking between two shapes
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
@@ -271,7 +282,7 @@ struct GJKSolver_indep
return false;
}
-
+ /// @brief intersection checking between one shape and a triangle
template<typename S>
bool shapeTriangleIntersect(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
@@ -315,6 +326,7 @@ struct GJKSolver_indep
return false;
}
+ //// @brief intersection checking between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleIntersect(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
@@ -358,7 +370,7 @@ struct GJKSolver_indep
return false;
}
-
+ /// @brief distance computation between two shapes
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const Transform3f& tf1,
const S2& s2, const Transform3f& tf2,
@@ -393,9 +405,7 @@ struct GJKSolver_indep
}
}
-
-
-
+ /// @brief distance computation between one shape and a triangle
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
@@ -431,6 +441,7 @@ struct GJKSolver_indep
}
}
+ /// @brief distance computation between one shape and a triangle with transformation
template<typename S>
bool shapeTriangleDistance(const S& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
@@ -466,7 +477,7 @@ struct GJKSolver_indep
}
}
-
+ /// @brief default setting for GJK algorithm
GJKSolver_indep()
{
gjk_max_iterations = 128;
@@ -477,49 +488,60 @@ struct GJKSolver_indep
epa_tolerance = 1e-6;
}
+ /// @brief maximum number of simplex face used in EPA algorithm
unsigned int epa_max_face_num;
+
+ /// @brief maximum number of simplex vertex used in EPA algorithm
unsigned int epa_max_vertex_num;
+
+ /// @brief maximum number of iterations used for EPA iterations
unsigned int epa_max_iterations;
+
+ /// @brief the threshold used in EPA to stop iteration
FCL_REAL epa_tolerance;
+
+ /// @brief the threshold used in GJK to stop iteration
FCL_REAL gjk_tolerance;
+
+ /// @brief maximum number of iterations used for GJK iterations
FCL_REAL gjk_max_iterations;
};
-/** \brief Fast implementation for sphere-sphere collision */
+/// @brief Fast implementation for sphere-sphere collision
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/** \brief Fast implementation for sphere-triangle collision */
+/// @brief Fast implementation for sphere-triangle collision
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/** \brief Fast implementation for sphere-triangle collision */
+/// @brief Fast implementation for sphere-triangle collision
template<>
bool GJKSolver_indep::shapeTriangleIntersect(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2, Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const;
-/** \brief Fast implementation for sphere-sphere distance */
+/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
FCL_REAL* dist) const;
-/** \brief Fast implementation for sphere-triangle distance */
+/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
FCL_REAL* dist) const;
-/** \brief Fast implementation for sphere-triangle distance */
+/// @brief Fast implementation for sphere-triangle distance
template<>
bool GJKSolver_indep::shapeTriangleDistance<Sphere>(const Sphere& s, const Transform3f& tf1,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
FCL_REAL* dist) const;
-/** \brief Fast implementation for box-box collision */
+/// @brief Fast implementation for box-box collision
template<>
bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f& tf1,
const Box& s2, const Transform3f& tf2,
diff --git a/trunk/fcl/include/fcl/octree.h b/trunk/fcl/include/fcl/octree.h
index 7960606e..48269add 100644
--- a/trunk/fcl/include/fcl/octree.h
+++ b/trunk/fcl/include/fcl/octree.h
@@ -49,21 +49,26 @@
namespace fcl
{
+/// @brief Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
class OcTree : public CollisionGeometry
{
private:
boost::shared_ptr<octomap::OcTree> tree;
public:
- // OcTreeNode must implement
- // 1) childExists(i)
- // 2) getChild(i)
- // 3) hasChildren()
+ /// @brief OcTreeNode must implement the following interfaces:
+ /// 1) childExists(i)
+ /// 2) getChild(i)
+ /// 3) hasChildren()
typedef octomap::OcTreeNode OcTreeNode;
+ /// @brief construct octree with a given resolution
OcTree(FCL_REAL resolution) : tree(boost::shared_ptr<octomap::OcTree>(new octomap::OcTree(resolution))) {}
+
+ /// @brief construct octree from octomap
OcTree(const boost::shared_ptr<octomap::OcTree>& tree_) : tree(tree_) {}
+ /// @brief compute the AABB for the octree in its local coordinate system
void computeLocalAABB()
{
aabb_local = getRootBV();
@@ -71,37 +76,45 @@ public:
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
+ /// @brief get the bounding volume for the root
inline AABB getRootBV() const
{
FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta));
}
+ /// @brief get the root node of the octree
inline OcTreeNode* getRoot() const
{
return tree->getRoot();
}
+ /// @brief whether one node is completely occupied
inline bool isNodeOccupied(const OcTreeNode* node) const
{
return tree->isNodeOccupied(node);
}
+ /// @brief whether one node is completely free
inline bool isNodeFree(const OcTreeNode* node) const
{
return false; // default no definitely free node
}
+ /// @brief whether one node is uncertain
inline bool isNodeUncertain(const OcTreeNode* node) const
{
return (!isNodeOccupied(node)) && (!isNodeFree(node));
}
+ /// @brief update the occupied information for a cell
inline void updateNode(FCL_REAL x, FCL_REAL y, FCL_REAL z, bool occupied)
{
tree->updateNode(octomap::point3d(x, y, z), occupied);
}
+ /// @brief transform the octree into a bunch of boxes; uncertainty information is kept in the boxes. However, we
+ /// only keep the occupied boxes (i.e., the boxes whose occupied probability is higher enough).
inline std::vector<boost::array<FCL_REAL, 6> > toBoxes() const
{
std::vector<boost::array<FCL_REAL, 6> > boxes;
@@ -126,15 +139,20 @@ public:
return boxes;
}
+ /// @brief the threshold used to decide whether one node is occupied
FCL_REAL getOccupancyThres() const
{
return tree->getOccupancyThres();
}
+ /// @brief return object type, it is an octree
OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
+
+ /// @brief return node type, it is an octree
NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
};
+/// @brief compute the bounding volume of an octree node's i-th child
static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv)
{
if(i&1)
diff --git a/trunk/fcl/include/fcl/profile.h b/trunk/fcl/include/fcl/profile.h
index f952bd09..f382d583 100644
--- a/trunk/fcl/include/fcl/profile.h
+++ b/trunk/fcl/include/fcl/profile.h
@@ -35,15 +35,14 @@
/* Author Ioan Sucan */
-#ifndef OMPL_UTIL_PROFILER_
-#define OMPL_UTIL_PROFILER_
+#ifndef FCL_UTIL_PROFILER
+#define FCL_UTIL_PROFILER
#define ENABLE_PROFILING 1
#ifndef ENABLE_PROFILING
-/** The ENABLE_PROFILING macro can be set externally. If it is not,
- profiling is enabled by default, unless NDEBUG is defined. */
+/// The ENABLE_PROFILING macro can be set externally. If it is not, profiling is enabled by default, unless NDEBUG is defined.
# ifdef NDEBUG
# define ENABLE_PROFILING 0
@@ -62,26 +61,26 @@
#include <boost/noncopyable.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
-namespace ompl
+namespace fcl
{
-/** \brief Namespace containing time datatypes and time operations */
+/// @brief Namespace containing time datatypes and time operations
namespace time
{
-/** \brief Representation of a point in time */
+/// @brief Representation of a point in time
typedef boost::posix_time::ptime point;
-/** \brief Representation of a time duration */
+/// @brief Representation of a time duration
typedef boost::posix_time::time_duration duration;
-/** \brief Get the current time point */
+/// @brief Get the current time point
inline point now(void)
{
return boost::posix_time::microsec_clock::universal_time();
}
-/** \brief Return the time duration representing a given number of seconds */
+/// @brief Return the time duration representing a given number of seconds
inline duration seconds(double sec)
{
long s = (long)sec;
@@ -89,7 +88,7 @@ inline duration seconds(double sec)
return boost::posix_time::seconds(s) + boost::posix_time::microseconds(us);
}
-/** \brief Return the number of seconds that a time duration represents */
+/// @brief Return the number of seconds that a time duration represents
inline double seconds(const duration &d)
{
return (double)d.total_microseconds() / 1000000.0;
@@ -100,20 +99,20 @@ inline double seconds(const duration &d)
namespace tools
{
-/** \brief This is a simple thread-safe tool for counting time
- spent in various chunks of code. This is different from
- external profiling tools in that it allows the user to count
- time spent in various bits of code (sub-function granularity)
- or count how many times certain pieces of code are executed.*/
+/// @brief This is a simple thread-safe tool for counting time
+/// spent in various chunks of code. This is different from
+/// external profiling tools in that it allows the user to count
+/// time spent in various bits of code (sub-function granularity)
+/// or count how many times certain pieces of code are executed.
class Profiler : private boost::noncopyable
{
public:
- /** \brief This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of scope. */
+ /// @brief This instance will call Profiler::begin() when constructed and Profiler::end() when it goes out of scope.
class ScopedBlock
{
public:
- /** \brief Start counting time for the block named \e name of the profiler \e prof */
+ /// @brief Start counting time for the block named \e name of the profiler \e prof
ScopedBlock(const std::string &name, Profiler &prof = Profiler::Instance()) : name_(name), prof_(prof)
{
prof_.begin(name);
@@ -130,13 +129,13 @@ public:
Profiler &prof_;
};
- /** \brief This instance will call Profiler::start() when constructed and Profiler::stop() when it goes out of scope.
- If the profiler was already started, this block's constructor and destructor take no action */
+ /// @brief This instance will call Profiler::start() when constructed and Profiler::stop() when it goes out of scope.
+ /// If the profiler was already started, this block's constructor and destructor take no action
class ScopedStart
{
public:
- /** \brief Take as argument the profiler instance to operate on (\e prof) */
+ /// @brief Take as argument the profiler instance to operate on (\e prof)
ScopedStart(Profiler &prof = Profiler::Instance()) : prof_(prof), wasRunning_(prof_.running())
{
if (!wasRunning_)
@@ -155,118 +154,107 @@ public:
bool wasRunning_;
};
- /** \brief Return an instance of the class */
+ /// @brief Return an instance of the class
static Profiler& Instance(void);
- /** \brief Constructor. It is allowed to separately instantiate this
- class (not only as a singleton) */
+ /// @brief Constructor. It is allowed to separately instantiate this
+ /// class (not only as a singleton)
Profiler(bool printOnDestroy = false, bool autoStart = false) : running_(false), printOnDestroy_(printOnDestroy)
{
if (autoStart)
start();
}
- /** \brief Destructor */
+ /// @brief Destructor
~Profiler(void)
{
if (printOnDestroy_ && !data_.empty())
status();
}
- /** \brief Start counting time */
+ /// @brief Start counting time
static void Start(void)
{
Instance().start();
}
- /** \brief Stop counting time */
+ /// @brief Stop counting time
static void Stop(void)
{
Instance().stop();
}
- /** \brief Clear counted time and events */
+ /// @brief Clear counted time and events
static void Clear(void)
{
Instance().clear();
}
- /** \brief Start counting time */
+ /// @brief Start counting time
void start(void);
- /** \brief Stop counting time */
+ /// @brief Stop counting time
void stop(void);
- /** \brief Clear counted time and events */
+ /// @brief Clear counted time and events
void clear(void);
- /** \brief Count a specific event for a number of times */
+ /// @brief Count a specific event for a number of times
static void Event(const std::string& name, const unsigned int times = 1)
{
Instance().event(name, times);
}
- /** \brief Count a specific event for a number of times */
+ /// @brief Count a specific event for a number of times
void event(const std::string &name, const unsigned int times = 1);
- /** \brief Maintain the average of a specific value */
+ /// @brief Maintain the average of a specific value
static void Average(const std::string& name, const double value)
{
Instance().average(name, value);
}
- /** \brief Maintain the average of a specific value */
+ /// @brief Maintain the average of a specific value
void average(const std::string &name, const double value);
- /** \brief Begin counting time for a specific chunk of code */
+ /// @brief Begin counting time for a specific chunk of code
static void Begin(const std::string &name)
{
Instance().begin(name);
}
- /** \brief Stop counting time for a specific chunk of code */
+ /// @brief Stop counting time for a specific chunk of code
static void End(const std::string &name)
{
Instance().end(name);
}
- /** \brief Begin counting time for a specific chunk of code */
+ /// @brief Begin counting time for a specific chunk of code
void begin(const std::string &name);
- /** \brief Stop counting time for a specific chunk of code */
+ /// @brief Stop counting time for a specific chunk of code
void end(const std::string &name);
- /** \brief Print the status of the profiled code chunks and
- events. Optionally, computation done by different threads
- can be printed separately. */
+ /// @brief Print the status of the profiled code chunks and
+ /// events. Optionally, computation done by different threads
+ /// can be printed separately.
static void Status(std::ostream &out = std::cout, bool merge = true)
{
Instance().status(out, merge);
}
- /** \brief Print the status of the profiled code chunks and
- events. Optionally, computation done by different threads
- can be printed separately. */
+ /// @brief Print the status of the profiled code chunks and
+ /// events. Optionally, computation done by different threads
+ /// can be printed separately.
void status(std::ostream &out = std::cout, bool merge = true);
- /** \brief Print the status of the profiled code chunks and
- events to the console (using msg::Console) */
- static void Console(void)
- {
- Instance().console();
- }
-
- /** \brief Print the status of the profiled code chunks and
- events to the console (using msg::Console) */
- void console(void);
-
- /** \brief Check if the profiler is counting time or not */
+ /// @brief Check if the profiler is counting time or not
bool running(void) const
{
return running_;
}
- /** \brief Check if the profiler is counting time or not */
+ /// @brief Check if the profiler is counting time or not
static bool Running(void)
{
return Instance().running();
@@ -274,35 +262,35 @@ public:
private:
- /** \brief Information about time spent in a section of the code */
+ /// @brief Information about time spent in a section of the code
struct TimeInfo
{
TimeInfo(void) : total(0, 0, 0, 0), shortest(boost::posix_time::pos_infin), longest(boost::posix_time::neg_infin), parts(0)
{
}
- /** \brief Total time counted. */
+ /// @brief Total time counted.
time::duration total;
- /** \brief The shortest counted time interval */
+ /// @brief The shortest counted time interval
time::duration shortest;
- /** \brief The longest counted time interval */
+ /// @brief The longest counted time interval
time::duration longest;
- /** \brief Number of times a chunk of time was added to this structure */
+ /// @brief Number of times a chunk of time was added to this structure
unsigned long int parts;
- /** \brief The point in time when counting time started */
+ /// @brief The point in time when counting time started
time::point start;
- /** \brief Begin counting time */
+ /// @brief Begin counting time
void set(void)
{
start = time::now();
}
- /** \brief Add the counted time to the total time */
+ /// @brief Add the counted time to the total time
void update(void)
{
const time::duration &dt = time::now() - start;
@@ -315,29 +303,29 @@ private:
}
};
- /** \brief Information maintained about averaged values */
+ /// @brief Information maintained about averaged values
struct AvgInfo
{
- /** \brief The sum of the values to average */
+ /// @brief The sum of the values to average
double total;
- /** \brief The sub of squares of the values to average */
+ /// @brief The sub of squares of the values to average
double totalSqr;
- /** \brief Number of times a value was added to this structure */
+ /// @brief Number of times a value was added to this structure
unsigned long int parts;
};
- /** \brief Information to be maintained for each thread */
+ /// @brief Information to be maintained for each thread
struct PerThread
{
- /** \brief The stored events */
+ /// @brief The stored events
std::map<std::string, unsigned long int> events;
- /** \brief The stored averages */
+ /// @brief The stored averages
std::map<std::string, AvgInfo> avg;
- /** \brief The amount of time spent in various places */
+ /// @brief The amount of time spent in various places
std::map<std::string, TimeInfo> time;
};
@@ -358,9 +346,8 @@ private:
#include <string>
#include <iostream>
-/* If profiling is disabled, provide empty implementations for the
- public functions */
-namespace ompl
+/// If profiling is disabled, provide empty implementations for the public functions
+namespace fcl
{
namespace tools
@@ -470,14 +457,6 @@ public:
{
}
- static void Console(void)
- {
- }
-
- void console(void)
- {
- }
-
bool running(void) const
{
return false;
diff --git a/trunk/fcl/include/fcl/math_simd_details.h b/trunk/fcl/include/fcl/simd/math_simd_details.h
similarity index 100%
rename from trunk/fcl/include/fcl/math_simd_details.h
rename to trunk/fcl/include/fcl/simd/math_simd_details.h
diff --git a/trunk/fcl/include/fcl/simd.h b/trunk/fcl/include/fcl/simd/simd.h
similarity index 97%
rename from trunk/fcl/include/fcl/simd.h
rename to trunk/fcl/include/fcl/simd/simd.h
index 1cb1e405..f63fd5e3 100644
--- a/trunk/fcl/include/fcl/simd.h
+++ b/trunk/fcl/include/fcl/simd/simd.h
@@ -37,6 +37,6 @@
#ifndef FCL_SIMD_H
#define FCL_SIMD_H
-#include "fcl/math_simd_details.h"
+#include "fcl/simd/math_simd_details.h"
#endif
diff --git a/trunk/fcl/include/fcl/simd_intersect.h b/trunk/fcl/include/fcl/simd/simd_intersect.h
similarity index 99%
rename from trunk/fcl/include/fcl/simd_intersect.h
rename to trunk/fcl/include/fcl/simd/simd_intersect.h
index d35d87df..b6e8dbbe 100644
--- a/trunk/fcl/include/fcl/simd_intersect.h
+++ b/trunk/fcl/include/fcl/simd/simd_intersect.h
@@ -37,7 +37,7 @@
#ifndef FCL_MULTIPLE_INTERSECT
#define FCL_MULTIPLE_INTERSECT
-#include "fcl/vec_3f.h"
+#include "fcl/math/vec_3f.h"
#include <xmmintrin.h>
#include <pmmintrin.h>
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal_node_base.h
index d7ca412b..c61ba68f 100644
--- a/trunk/fcl/include/fcl/traversal_node_base.h
+++ b/trunk/fcl/include/fcl/traversal_node_base.h
@@ -38,13 +38,13 @@
#define FCL_TRAVERSAL_NODE_BASE_H
#include "fcl/data_types.h"
-#include "fcl/transform.h"
+#include "fcl/math/transform.h"
#include "fcl/collision_data.h"
-/** \brief Main namespace */
namespace fcl
{
+/// @brief Node structure encoding the information required for traversal.
class TraversalNodeBase
{
public:
@@ -54,35 +54,38 @@ public:
virtual void postprocess() {}
- /** \brief Whether b is a leaf node in the first BVH tree */
+ /// @brief Whether b is a leaf node in the first BVH tree
virtual bool isFirstNodeLeaf(int b) const;
- /** \brief Whether b is a leaf node in the second BVH tree */
+ /// @brief Whether b is a leaf node in the second BVH tree
virtual bool isSecondNodeLeaf(int b) const;
- /** \brief Traverse the subtree of the node in the first tree first */
+ /// @brief Traverse the subtree of the node in the first tree first
virtual bool firstOverSecond(int b1, int b2) const;
- /** \brief Get the left child of the node b in the first tree */
+ /// @brief Get the left child of the node b in the first tree
virtual int getFirstLeftChild(int b) const;
- /** \brief Get the right child of the node b in the first tree */
+ /// @brief Get the right child of the node b in the first tree
virtual int getFirstRightChild(int b) const;
- /** \brief Get the left child of the node b in the second tree */
+ /// @brief Get the left child of the node b in the second tree
virtual int getSecondLeftChild(int b) const;
- /** \brief Get the right child of the node b in the second tree */
+ /// @brief Get the right child of the node b in the second tree
virtual int getSecondRightChild(int b) const;
- /** \brief Enable statistics (verbose mode) */
+ /// @brief Enable statistics (verbose mode)
virtual void enableStatistics(bool enable) = 0;
+ /// @brief configuation of first object
Transform3f tf1;
+ /// @brief configuration of second object
Transform3f tf2;
};
+/// @brief Node structure encoding the information required for collision traversal.
class CollisionTraversalNodeBase : public TraversalNodeBase
{
public:
@@ -90,24 +93,29 @@ public:
virtual ~CollisionTraversalNodeBase();
- /** \brief BV test between b1 and b2 */
+ /// @brief BV test between b1 and b2
virtual bool BVTesting(int b1, int b2) const;
- /** \brief Leaf test between node b1 and b2, if they are both leafs */
+ /// @brief Leaf test between node b1 and b2, if they are both leafs
virtual void leafTesting(int b1, int b2) const;
- /** \brief Check whether the traversal can stop */
+ /// @brief Check whether the traversal can stop
virtual bool canStop() const;
+ /// @brief Whether store some statistics information during traversal
void enableStatistics(bool enable) { enable_statistics = enable; }
+ /// @brief request setting for collision
CollisionRequest request;
+ /// @brief collision result kept during the traversal iteration
CollisionResult* result;
+ /// @brief Whether stores statistics
bool enable_statistics;
};
+/// @brief Node structure encoding the information required for distance traversal.
class DistanceTraversalNodeBase : public TraversalNodeBase
{
public:
@@ -115,18 +123,25 @@ public:
virtual ~DistanceTraversalNodeBase();
+ /// @brief BV test between b1 and b2
virtual FCL_REAL BVTesting(int b1, int b2) const;
+ /// @brief Leaf test between node b1 and b2, if they are both leafs
virtual void leafTesting(int b1, int b2) const;
+ /// @brief Check whether the traversal can stop
virtual bool canStop(FCL_REAL c) const;
+ /// @brief Whether store some statistics information during traversal
void enableStatistics(bool enable) { enable_statistics = enable; }
+ /// @brief request setting for distance
DistanceRequest request;
+ /// @brief distance result kept during the traversal iteration
DistanceResult* result;
+ /// @brief Whether stores statistics
bool enable_statistics;
};
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index b03905e5..73010b8a 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -47,6 +47,7 @@
namespace fcl
{
+/// @brief Traversal node for collision between BVH and shape
template<typename BV, typename S>
class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
@@ -61,21 +62,25 @@ public:
query_time_seconds = 0.0;
}
+ /// @brief Whether the BV node in the first BVH tree is leaf
bool isFirstNodeLeaf(int b) const
{
return model1->getBV(b).isLeaf();
}
+ /// @brief Obtain the left child of BV node in the first BVH
int getFirstLeftChild(int b) const
{
return model1->getBV(b).leftChild();
}
+ /// @brief Obtain the right child of BV node in the first BVH
int getFirstRightChild(int b) const
{
return model1->getBV(b).rightChild();
}
+ /// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) num_bv_tests++;
@@ -91,7 +96,7 @@ public:
mutable FCL_REAL query_time_seconds;
};
-
+/// @brief Traversal node for collision between shape and BVH
template<typename S, typename BV>
class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase
{
@@ -106,26 +111,31 @@ public:
query_time_seconds = 0.0;
}
+ /// @brief Alway extend the second model, which is a BVH model
bool firstOverSecond(int, int) const
{
return false;
}
+ /// @brief Whether the BV node in the second BVH tree is leaf
bool isSecondNodeLeaf(int b) const
{
return model2->getBV(b).isLeaf();
}
+ /// @brief Obtain the left child of BV node in the second BVH
int getSecondLeftChild(int b) const
{
return model2->getBV(b).leftChild();
}
+ /// @brief Obtain the right child of BV node in the second BVH
int getSecondRightChild(int b) const
{
return model2->getBV(b).rightChild();
}
+ /// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int b2) const
{
if(this->enable_statistics) num_bv_tests++;
@@ -142,7 +152,7 @@ public:
};
-
+/// @brief Traversal node for collision between mesh and shape
template<typename BV, typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
{
@@ -155,6 +165,7 @@ public:
nsolver = NULL;
}
+ /// @brief Intersection testing between leaves (one triangle and one shape)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -201,7 +212,7 @@ public:
AABB shape_aabb;
computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
- this->result->addCostSource(CostSource(overlap_part, cost_density));
+ this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
@@ -212,14 +223,15 @@ public:
AABB shape_aabb;
computeBV<AABB, S>(*(this->model2), this->tf2, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
- this->result->addCostSource(CostSource(overlap_part, cost_density));
+ this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
}
+ /// @brief Whether the traversal process can stop early
bool canStop() const
{
- return (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts());
+ return this->request.isSatisfied(*(this->result));
}
Vec3f* vertices;
@@ -230,7 +242,7 @@ public:
const NarrowPhaseSolver* nsolver;
};
-
+/// @cond IGNORE
namespace details
{
template<typename BV, typename S, typename NarrowPhaseSolver>
@@ -290,7 +302,7 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
AABB shape_aabb;
computeBV<AABB, S>(model2, tf2, shape_aabb);
bool res = AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part);
- result.addCostSource(CostSource(overlap_part, cost_density));
+ result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
else if((!model1->isFree() || model2.isFree()) && request.enable_cost)
@@ -301,13 +313,17 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
AABB shape_aabb;
computeBV<AABB, S>(model2, tf2, shape_aabb);
bool res = AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(shape_aabb, overlap_part);
- result.addCostSource(CostSource(overlap_part, cost_density));
+ result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
}
}
+/// @endcond
+
+
+/// @brief Traversal node for mesh and shape, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
{
@@ -396,6 +412,8 @@ public:
};
+
+/// @brief Traversal node for collision between shape and mesh
template<typename S, typename BV, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode<S, BV>
{
@@ -408,6 +426,7 @@ public:
nsolver = NULL;
}
+ /// @brief Intersection testing between leaves (one shape and one triangle)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -454,7 +473,7 @@ public:
AABB shape_aabb;
computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
- this->result->addCostSource(CostSource(overlap_part, cost_density));
+ this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
@@ -465,14 +484,15 @@ public:
AABB shape_aabb;
computeBV<AABB, S>(*(this->model1), this->tf1, shape_aabb);
AABB(p1, p2, p3).overlap(shape_aabb, overlap_part);
- this->result->addCostSource(CostSource(overlap_part, cost_density));
+ this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
}
+ /// @brief Whether the traversal process can stop early
bool canStop() const
{
- return (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts());
+ return this->request.isSatisfied(*(this->result));
}
Vec3f* vertices;
@@ -483,6 +503,7 @@ public:
const NarrowPhaseSolver* nsolver;
};
+/// @brief Traversal node for shape and mesh, when mesh BVH is one of the oriented node (OBB, RSS, OBBRSS, kIOS)
template<typename S, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>
{
@@ -581,7 +602,7 @@ public:
};
-
+/// @brief Traversal node for distance computation between BVH and shape
template<typename BV, typename S>
class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase
{
@@ -596,21 +617,25 @@ public:
query_time_seconds = 0.0;
}
+ /// @brief Whether the BV node in the first BVH tree is leaf
bool isFirstNodeLeaf(int b) const
{
return model1->getBV(b).isLeaf();
}
+ /// @brief Obtain the left child of BV node in the first BVH
int getFirstLeftChild(int b) const
{
return model1->getBV(b).leftChild();
}
+ /// @brief Obtain the right child of BV node in the first BVH
int getFirstRightChild(int b) const
{
return model1->getBV(b).rightChild();
}
+ /// @brief BV culling test in one BVTT node
FCL_REAL BVTesting(int b1, int b2) const
{
return model1->getBV(b1).bv.distance(model2_bv);
@@ -625,6 +650,7 @@ public:
mutable FCL_REAL query_time_seconds;
};
+/// @brief Traversal node for distance computation between shape and BVH
template<typename S, typename BV>
class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase
{
@@ -639,21 +665,25 @@ public:
query_time_seconds = 0.0;
}
+ /// @brief Whether the BV node in the second BVH tree is leaf
bool isSecondNodeLeaf(int b) const
{
return model2->getBV(b).isLeaf();
}
+ /// @brief Obtain the left child of BV node in the second BVH
int getSecondLeftChild(int b) const
{
return model2->getBV(b).leftChild();
}
+ /// @brief Obtain the right child of BV node in the second BVH
int getSecondRightChild(int b) const
{
return model2->getBV(b).rightChild();
}
+ /// @brief BV culling test in one BVTT node
FCL_REAL BVTesting(int b1, int b2) const
{
return model1_bv.distance(model2->getBV(b2).bv);
@@ -669,7 +699,7 @@ public:
};
-
+/// @brief Traversal node for distance between mesh and shape
template<typename BV, typename S, typename NarrowPhaseSolver>
class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode<BV, S>
{
@@ -685,6 +715,7 @@ public:
nsolver = NULL;
}
+ /// @brief Distance testing between leaves (one triangle and one shape)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -705,6 +736,7 @@ public:
this->result->update(d, this->model1, this->model2, primitive_id, DistanceResult::NONE);
}
+ /// @brief Whether the traversal process can stop early
bool canStop(FCL_REAL c) const
{
if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
@@ -721,6 +753,7 @@ public:
const NarrowPhaseSolver* nsolver;
};
+/// @cond IGNORE
namespace details
{
@@ -752,6 +785,7 @@ void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE);
}
+
template<typename BV, typename S, typename NarrowPhaseSolver>
static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
Vec3f* vertices, Triangle* tri_indices, int init_tri_id,
@@ -775,9 +809,11 @@ static inline void distancePreprocessOrientedNode(const BVHModel<BV>* model1,
}
+/// @endcond
+/// @brief Traversal node for distance between mesh and shape, when mesh BVH is one of the oriented node (RSS, OBBRSS, kIOS)
template<typename S, typename NarrowPhaseSolver>
class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<RSS, S, NarrowPhaseSolver>
{
@@ -875,7 +911,7 @@ public:
};
-
+/// @brief Traversal node for distance between shape and mesh
template<typename S, typename BV, typename NarrowPhaseSolver>
class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode<S, BV>
{
@@ -891,6 +927,7 @@ public:
nsolver = NULL;
}
+ /// @brief Distance testing between leaves (one shape and one triangle)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -911,6 +948,7 @@ public:
this->result->update(distance, this->model1, this->model2, DistanceResult::NONE, primitive_id);
}
+ /// @brief Whether the traversal process can stop early
bool canStop(FCL_REAL c) const
{
if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index 19ad2120..e43f7e94 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -44,7 +44,7 @@
#include "fcl/BV.h"
#include "fcl/BVH_model.h"
#include "fcl/intersect.h"
-#include "fcl/motion.h"
+#include "fcl/ccd/motion.h"
#include <boost/shared_array.hpp>
#include <boost/shared_ptr.hpp>
@@ -52,11 +52,11 @@
#include <vector>
#include <cassert>
-/** \brief Main namespace */
+
namespace fcl
{
-/** \brief Traversal node for collision between BVH models */
+/// @brief Traversal node for collision between BVH models
template<typename BV>
class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
{
@@ -71,19 +71,19 @@ public:
query_time_seconds = 0.0;
}
- /** \brief Whether the BV node in the first BVH tree is leaf */
+ /// @brief Whether the BV node in the first BVH tree is leaf
bool isFirstNodeLeaf(int b) const
{
return model1->getBV(b).isLeaf();
}
- /** \brief Whether the BV node in the second BVH tree is leaf */
+ /// @brief Whether the BV node in the second BVH tree is leaf
bool isSecondNodeLeaf(int b) const
{
return model2->getBV(b).isLeaf();
}
- /** \brief Determine the traversal order, is the first BVTT subtree better */
+ /// @brief Determine the traversal order, is the first BVTT subtree better
bool firstOverSecond(int b1, int b2) const
{
FCL_REAL sz1 = model1->getBV(b1).bv.size();
@@ -97,43 +97,43 @@ public:
return false;
}
- /** \brief Obtain the left child of BV node in the first BVH */
+ /// @brief Obtain the left child of BV node in the first BVH
int getFirstLeftChild(int b) const
{
return model1->getBV(b).leftChild();
}
- /** \brief Obtain the right child of BV node in the first BVH */
+ /// @brief Obtain the right child of BV node in the first BVH
int getFirstRightChild(int b) const
{
return model1->getBV(b).rightChild();
}
- /** \brief Obtain the left child of BV node in the second BVH */
+ /// @brief Obtain the left child of BV node in the second BVH
int getSecondLeftChild(int b) const
{
return model2->getBV(b).leftChild();
}
- /** \brief Obtain the right child of BV node in the second BVH */
+ /// @brief Obtain the right child of BV node in the second BVH
int getSecondRightChild(int b) const
{
return model2->getBV(b).rightChild();
}
- /** \brief BV culling test in one BVTT node */
+ /// @brief BV culling test in one BVTT node
bool BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return !model1->getBV(b1).overlap(model2->getBV(b2));
}
- /** \brief The first BVH model */
+ /// @brief The first BVH model
const BVHModel<BV>* model1;
- /** \brief The second BVH model */
+ /// @brief The second BVH model
const BVHModel<BV>* model2;
- /** \brief statistical information */
+ /// @brief statistical information
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
@@ -141,7 +141,7 @@ public:
};
-/** \brief Traversal node for collision between two meshes */
+/// @brief Traversal node for collision between two meshes
template<typename BV>
class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
{
@@ -154,7 +154,7 @@ public:
tri_indices2 = NULL;
}
- /** \brief Intersection testing between leaves (two triangles) */
+ /// @brief Intersection testing between leaves (two triangles)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -217,7 +217,7 @@ public:
{
AABB overlap_part;
AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
- this->result->addCostSource(CostSource(overlap_part, cost_density));
+ this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
else if((!this->model1->isFree() && !this->model2->isFree()) && this->request.enable_cost)
@@ -226,15 +226,15 @@ public:
{
AABB overlap_part;
AABB(p1, p2, p3).overlap(AABB(q1, q2, q3), overlap_part);
- this->result->addCostSource(CostSource(overlap_part, cost_density));
+ this->result->addCostSource(CostSource(overlap_part, cost_density), this->request.num_max_cost_sources);
}
}
}
- /** \brief Whether the traversal process can stop early */
+ /// @brief Whether the traversal process can stop early
bool canStop() const
{
- return (!this->request.enable_cost) && (this->result->isCollision()) && (this->request.num_max_contacts <= this->result->numContacts());
+ return this->request.isSatisfied(*(this->result));
}
Vec3f* vertices1;
@@ -247,7 +247,7 @@ public:
};
-
+/// @brief Traversal node for collision between two meshes if their underlying BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
{
public:
@@ -265,7 +265,6 @@ public:
Vec3f T;
};
-
class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
{
public:
@@ -309,246 +308,24 @@ public:
Vec3f T;
};
-#if USE_SVMLIGHT
-
-struct BVHPointCollisionPair
-{
- BVHPointCollisionPair() {}
-
- BVHPointCollisionPair(int id1_start_, int id1_num_, int id2_start_, int id2_num_, FCL_REAL collision_prob_)
- : id1_start(id1_start_), id1_num(id1_num_), id2_start(id2_start_), id2_num(id2_num_), collision_prob(collision_prob_) {}
-
- int id1_start;
- int id1_num;
-
- int id2_start;
- int id2_num;
-
- FCL_REAL collision_prob;
-};
-
-struct BVHPointCollisionPairComp
-{
- bool operator()(const BVHPointCollisionPair& a, const BVHPointCollisionPair& b)
- {
- if(a.id1_start == b.id1_start)
- return a.id2_start < b.id2_start;
- return a.id1_start < b.id1_start;
- }
-};
-
-
-template<typename BV>
-class PointCloudCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- PointCloudCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
-
- collision_prob_threshold = 0.5;
- max_collision_prob = 0;
- leaf_size_threshold = 1;
- }
-
- bool isFirstNodeLeaf(int b) const
- {
- const BVNode<BV>& node = this->model1->getBV(b);
- return ((node.num_primitives < leaf_size_threshold) || node.isLeaf());
- }
-
- bool isSecondNodeLeaf(int b) const
- {
- const BVNode<BV>& node = this->model2->getBV(b);
- return ((node.num_primitives < leaf_size_threshold) || node.isLeaf());
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- FCL_REAL collision_prob = Intersect::intersect_PointClouds(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- vertices2 + node2.first_primitive, uc2.get() + node2.first_primitive,
- node2.num_primitives,
- classifier_param);
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
-
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- boost::shared_array<Variance3f> uc1;
- boost::shared_array<Variance3f> uc2;
-
- mutable std::vector<BVHPointCollisionPair> pairs;
-
- int leaf_size_threshold;
-
- FCL_REAL collision_prob_threshold;
-
- mutable FCL_REAL max_collision_prob;
-
- CloudClassifierParam classifier_param;
-};
-
-
-
-class PointCloudCollisionTraversalNodeOBB : public PointCloudCollisionTraversalNode<OBB>
-{
-public:
- PointCloudCollisionTraversalNodeOBB();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Matrix3f R;
- Vec3f T;
-};
-
-
-class PointCloudCollisionTraversalNodeRSS : public PointCloudCollisionTraversalNode<RSS>
-{
-public:
- PointCloudCollisionTraversalNodeRSS();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Matrix3f R;
- Vec3f T;
-};
-
-
-
-template<typename BV>
-class PointCloudMeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- PointCloudMeshCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices2 = NULL;
-
- collision_prob_threshold = 0.5;
- max_collision_prob = 0;
- leaf_size_threshold = 1;
- }
-
- bool isFirstNodeLeaf(int b) const
- {
- const BVNode<BV>& node = this->model1->getBV(b);
- return ((node.num_primitives < leaf_size_threshold) || node.isLeaf());
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- const Triangle& tri_id2 = tri_indices2[node2.primitiveId()];
-
- const Vec3f& q1 = vertices2[tri_id2[0]];
- const Vec3f& q2 = vertices2[tri_id2[1]];
- const Vec3f& q3 = vertices2[tri_id2[2]];
-
- FCL_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- q1, q2, q3);
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- boost::shared_array<Variance3f> uc1;
- Triangle* tri_indices2;
-
- mutable std::vector<BVHPointCollisionPair> pairs;
-
- int leaf_size_threshold;
-
- FCL_REAL collision_prob_threshold;
-
- mutable FCL_REAL max_collision_prob;
-};
-
-
-class PointCloudMeshCollisionTraversalNodeOBB : public PointCloudMeshCollisionTraversalNode<OBB>
-{
-public:
- PointCloudMeshCollisionTraversalNodeOBB();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Matrix3f R;
- Vec3f T;
-};
-
-class PointCloudMeshCollisionTraversalNodeRSS : public PointCloudMeshCollisionTraversalNode<RSS>
-{
-public:
- PointCloudMeshCollisionTraversalNodeRSS();
-
- bool BVTesting(int b1, int b2) const;
-
- void leafTesting(int b1, int b2) const;
-
- Matrix3f R;
- Vec3f T;
-};
-
-#endif
-
+/// @brief Traversal node for continuous collision between BVH models
struct BVHContinuousCollisionPair
{
BVHContinuousCollisionPair() {}
BVHContinuousCollisionPair(int id1_, int id2_, FCL_REAL time) : id1(id1_), id2(id2_), collision_time(time) {}
- /** \brief The index of one in-collision primitive */
+ /// @brief The index of one in-collision primitive
int id1;
- /** \brief The index of the other in-collision primitive */
+ /// @brief The index of the other in-collision primitive
int id2;
- /** \brief Collision time normalized in [0, 1]. The collision time out of [0, 1] means collision-free */
+ /// @brief Collision time normalized in [0, 1]. The collision time out of [0, 1] means collision-free
FCL_REAL collision_time;
};
-
+/// @brief Traversal node for continuous collision between meshes
template<typename BV>
class MeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
{
@@ -566,6 +343,7 @@ public:
num_ee_tests = 0;
}
+ /// @brief Intersection testing between leaves (two triangles)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -648,6 +426,7 @@ public:
pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
}
+ /// @brief Whether the traversal process can stop early
bool canStop() const
{
return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size());
@@ -669,174 +448,8 @@ public:
};
-template<typename BV>
-class MeshPointCloudContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- MeshPointCloudContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices1 = NULL;
- prev_vertices1 = NULL;
- prev_vertices2 = NULL;
-
- num_vf_tests = 0;
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- FCL_REAL collision_time = 2;
- Vec3f collision_pos;
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- const Triangle& tri_id1 = tri_indices1[primitive_id1];
- int vertex_id2 = primitive_id2;
-
- Vec3f* S0[3];
- Vec3f* S1[3];
-
- for(int i = 0; i < 3; ++i)
- {
- S0[i] = prev_vertices1 + tri_id1[i];
- S1[i] = vertices1 + tri_id1[i];
- }
- Vec3f* T0 = prev_vertices2 + vertex_id2;
- Vec3f* T1 = vertices2 + vertex_id2;
-
- FCL_REAL tmp;
- Vec3f tmpv;
-
- // 3 VF checks
- for(int i = 0; i < 3; ++i)
- {
- num_vf_tests++;
- if(Intersect::intersect_VF(*(S0[0]), *(S0[1]), *(S0[2]), *T0, *(S1[0]), *(S1[1]), *(S1[2]), *T1, &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
- }
-
- if(!(collision_time > 1)) // collision happens
- {
- pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
- }
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- Triangle* tri_indices1;
-
- Vec3f* prev_vertices1;
- Vec3f* prev_vertices2;
-
- mutable int num_vf_tests;
-
- mutable std::vector<BVHContinuousCollisionPair> pairs;
-};
-
-
-template<typename BV>
-class PointCloudMeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
-{
-public:
- PointCloudMeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
- {
- vertices1 = NULL;
- vertices2 = NULL;
- tri_indices2 = NULL;
- prev_vertices1 = NULL;
- prev_vertices2 = NULL;
-
- num_vf_tests = 0;
- }
-
- void leafTesting(int b1, int b2) const
- {
- if(this->enable_statistics) this->num_leaf_tests++;
-
- const BVNode<BV>& node1 = this->model1->getBV(b1);
- const BVNode<BV>& node2 = this->model2->getBV(b2);
-
- FCL_REAL collision_time = 2;
- Vec3f collision_pos;
-
- int primitive_id1 = node1.primitiveId();
- int primitive_id2 = node2.primitiveId();
-
- int vertex_id1 = primitive_id1;
- const Triangle& tri_id2 = tri_indices2[primitive_id2];
-
- Vec3f* S0 = prev_vertices1 + vertex_id1;
- Vec3f* S1 = vertices1 + vertex_id1;
-
- Vec3f* T0[3];
- Vec3f* T1[3];
- for(int i = 0; i < 3; ++i)
- {
- T0[i] = prev_vertices2 + tri_id2[i];
- T1[i] = vertices2 + tri_id2[i];
- }
-
- FCL_REAL tmp;
- Vec3f tmpv;
-
- // 3 VF checks
- for(int i = 0; i < 3; ++i)
- {
- num_vf_tests++;
- if(Intersect::intersect_VF(*(T0[0]), *(T0[1]), *(T0[2]), *S0, *(T1[0]), *(T1[1]), *(T1[2]), *S1, &tmp, &tmpv))
- {
- if(collision_time > tmp)
- {
- collision_time = tmp; collision_pos = tmpv;
- }
- }
- }
-
- if(!(collision_time > 1)) // collision happens
- {
- pairs.push_back(BVHContinuousCollisionPair(primitive_id1, primitive_id2, collision_time));
- }
- }
-
- bool canStop() const
- {
- return (pairs.size() > 0) && (this->request.num_max_contacts <= pairs.size());
- }
-
- Vec3f* vertices1;
- Vec3f* vertices2;
-
- Triangle* tri_indices2;
-
- Vec3f* prev_vertices1;
- Vec3f* prev_vertices2;
-
- mutable int num_vf_tests;
-
- mutable std::vector<BVHContinuousCollisionPair> pairs;
-};
-
-
-
+/// @brief Traversal node for distance computation between BVH models
template<typename BV>
class BVHDistanceTraversalNode : public DistanceTraversalNodeBase
{
@@ -851,16 +464,19 @@ public:
query_time_seconds = 0.0;
}
+ /// @brief Whether the BV node in the first BVH tree is leaf
bool isFirstNodeLeaf(int b) const
{
return model1->getBV(b).isLeaf();
}
+ /// @brief Whether the BV node in the second BVH tree is leaf
bool isSecondNodeLeaf(int b) const
{
return model2->getBV(b).isLeaf();
}
+ /// @brief Determine the traversal order, is the first BVTT subtree better
bool firstOverSecond(int b1, int b2) const
{
FCL_REAL sz1 = model1->getBV(b1).bv.size();
@@ -874,43 +490,50 @@ public:
return false;
}
+ /// @brief Obtain the left child of BV node in the first BVH
int getFirstLeftChild(int b) const
{
return model1->getBV(b).leftChild();
}
+ /// @brief Obtain the right child of BV node in the first BVH
int getFirstRightChild(int b) const
{
return model1->getBV(b).rightChild();
}
+ /// @brief Obtain the left child of BV node in the second BVH
int getSecondLeftChild(int b) const
{
return model2->getBV(b).leftChild();
}
+ /// @brief Obtain the right child of BV node in the second BVH
int getSecondRightChild(int b) const
{
return model2->getBV(b).rightChild();
}
+ /// @brief BV culling test in one BVTT node
FCL_REAL BVTesting(int b1, int b2) const
{
if(enable_statistics) num_bv_tests++;
return model1->getBV(b1).distance(model2->getBV(b2));
}
+ /// @brief The first BVH model
const BVHModel<BV>* model1;
+ /// @brief The second BVH model
const BVHModel<BV>* model2;
+ /// @brief statistical information
mutable int num_bv_tests;
mutable int num_leaf_tests;
mutable FCL_REAL query_time_seconds;
-
};
-
+/// @brief Traversal node for distance computation between two meshes
template<typename BV>
class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV>
{
@@ -926,6 +549,7 @@ public:
abs_err = 0;
}
+ /// @brief Distance testing between leaves (two triangles)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -961,8 +585,9 @@ public:
{
this->result->update(d, this->model1, this->model2, primitive_id1, primitive_id2);
}
- }
+ }
+ /// @brief Whether the traversal process can stop early
bool canStop(FCL_REAL c) const
{
if((c >= this->result->min_distance - abs_err) && (c * (1 + rel_err) >= this->result->min_distance))
@@ -976,12 +601,12 @@ public:
Triangle* tri_indices1;
Triangle* tri_indices2;
- /** \brief relative and absolute error, default value is 0.01 for both terms */
+ /// @brief relative and absolute error, default value is 0.01 for both terms
FCL_REAL rel_err;
FCL_REAL abs_err;
};
-
+/// @brief Traversal node for distance computation between two meshes if their underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
class MeshDistanceTraversalNodeRSS : public MeshDistanceTraversalNode<RSS>
{
public:
@@ -1046,7 +671,7 @@ struct ConservativeAdvancementStackData
FCL_REAL d;
};
-// when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration
+/// @brief continuous collision node using conservative advancement. when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration
template<typename BV>
class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode<BV>
{
@@ -1063,6 +688,7 @@ public:
motion2 = NULL;
}
+ /// @brief BV culling test in one BVTT node
FCL_REAL BVTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_bv_tests++;
@@ -1074,6 +700,7 @@ public:
return d;
}
+ /// @brief Conservative advancement testing between leaves (two triangles)
void leafTesting(int b1, int b2) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -1129,7 +756,7 @@ public:
delta_t = cur_delta_t;
}
-
+ /// @brief Whether the traversal process can stop early
bool canStop(FCL_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
@@ -1194,17 +821,17 @@ public:
mutable int last_tri_id1, last_tri_id2;
- /** \brief CA controlling variable: early stop for the early iterations of CA */
+ /// @brief CA controlling variable: early stop for the early iterations of CA
FCL_REAL w;
- /** \brief The time from beginning point */
+ /// @brief The time from beginning point
FCL_REAL toc;
FCL_REAL t_err;
- /** \brief The delta_t each step */
+ /// @brief The delta_t each step
mutable FCL_REAL delta_t;
- /** \brief Motions for the two objects in query */
+ /// @brief Motions for the two objects in query
MotionBase<BV>* motion1;
MotionBase<BV>* motion2;
@@ -1212,7 +839,7 @@ public:
};
-/** for OBB and RSS, there is local coordinate of BV, so normal need to be transformed */
+/// @brief for OBB and RSS, there is local coordinate of BV, so normal need to be transformed
template<>
bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const;
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h
index 0125cf85..779df4a4 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal_node_octree.h
@@ -47,7 +47,7 @@
namespace fcl
{
-
+/// @brief Algorithms for collision related with octree
template<typename NarrowPhaseSolver>
class OcTreeSolver
{
@@ -69,6 +69,7 @@ public:
{
}
+ /// @brief collision between two octrees
void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
const Transform3f& tf1, const Transform3f& tf2,
const CollisionRequest& request_,
@@ -82,7 +83,7 @@ public:
tf1, tf2);
}
-
+ /// @brief distance between two octrees
void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
const Transform3f& tf1, const Transform3f& tf2,
const DistanceRequest& request_,
@@ -96,6 +97,7 @@ public:
tf1, tf2);
}
+ /// @brief collision between octree and mesh
template<typename BV>
void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
const Transform3f& tf1, const Transform3f& tf2,
@@ -110,6 +112,7 @@ public:
tf1, tf2);
}
+ /// @brief distance between octree and mesh
template<typename BV>
void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
const Transform3f& tf1, const Transform3f& tf2,
@@ -124,7 +127,7 @@ public:
tf1, tf2);
}
-
+ /// @brief collision between mesh and octree
template<typename BV>
void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
const Transform3f& tf1, const Transform3f& tf2,
@@ -140,7 +143,7 @@ public:
tf2, tf1);
}
-
+ /// @brief distance between mesh and octree
template<typename BV>
void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
const Transform3f& tf1, const Transform3f& tf2,
@@ -155,6 +158,7 @@ public:
tf1, tf2);
}
+ /// @brief collision between octree and shape
template<typename S>
void OcTreeShapeIntersect(const OcTree* tree, const S& s,
const Transform3f& tf1, const Transform3f& tf2,
@@ -174,6 +178,7 @@ public:
}
+ /// @brief collision between shape and octree
template<typename S>
void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
const Transform3f& tf1, const Transform3f& tf2,
@@ -192,6 +197,7 @@ public:
tf2, tf1);
}
+ /// @brief distance between octree and shape
template<typename S>
void OcTreeShapeDistance(const OcTree* tree, const S& s,
const Transform3f& tf1, const Transform3f& tf2,
@@ -208,6 +214,7 @@ public:
tf1, tf2);
}
+ /// @brief distance between shape and octree
template<typename S>
void ShapeOcTreeDistance(const S& s, const OcTree* tree,
const Transform3f& tf1, const Transform3f& tf2,
@@ -244,7 +251,7 @@ private:
dresult->update(dist, tree1, &s, root1 - tree1->getRoot(), DistanceResult::NONE);
- return (dresult->min_distance <= 0);
+ return drequest->isSatisfied(*dresult);
}
else
return false;
@@ -322,10 +329,10 @@ private:
computeBV<AABB, Box>(box, box_tf, aabb1);
computeBV<AABB, S>(s, tf2, aabb2);
aabb1.overlap(aabb2, overlap_part);
- cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density));
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density), crequest->num_max_cost_sources);
}
- return (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts());
+ return crequest->isSatisfied(*cresult);
}
else return false;
}
@@ -346,7 +353,7 @@ private:
computeBV<AABB, Box>(box, box_tf, aabb1);
computeBV<AABB, S>(s, tf2, aabb2);
aabb1.overlap(aabb2, overlap_part);
- cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density));
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * s.cost_density), crequest->num_max_cost_sources);
}
}
@@ -408,7 +415,7 @@ private:
dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), primitive_id);
- return (dresult->min_distance <= 0);
+ return drequest->isSatisfied(*dresult);
}
else
return false;
@@ -525,10 +532,10 @@ private:
computeBV<AABB, Box>(box, box_tf, aabb1);
AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
aabb1.overlap(aabb2, overlap_part);
- cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density));
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources);
}
- return (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts());
+ return crequest->isSatisfied(*cresult);
}
else
return false;
@@ -557,7 +564,7 @@ private:
computeBV<AABB, Box>(box, box_tf, aabb1);
AABB aabb2(tf2.transform(p1), tf2.transform(p2), tf2.transform(p3));
aabb1.overlap(aabb2, overlap_part);
- cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density));
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * tree2->cost_density), crequest->num_max_cost_sources);
}
}
@@ -626,7 +633,7 @@ private:
dresult->update(dist, tree1, tree2, root1 - tree1->getRoot(), root2 - tree2->getRoot());
- return (dresult->min_distance <= 0);
+ return drequest->isSatisfied(*dresult);
}
else
return false;
@@ -740,10 +747,10 @@ private:
computeBV<AABB, Box>(box1, box1_tf, aabb1);
computeBV<AABB, Box>(box2, box2_tf, aabb2);
aabb1.overlap(aabb2, overlap_part);
- cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()));
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
}
- return (!crequest->enable_cost) && (cresult->isCollision()) && (crequest->num_max_contacts <= cresult->numContacts());
+ return crequest->isSatisfied(*cresult);
}
else if(!tree1->isNodeFree(root1) && !tree2->isNodeFree(root2) && crequest->enable_cost) // uncertain area (here means both are uncertain or one uncertain and one occupied)
{
@@ -763,7 +770,7 @@ private:
computeBV<AABB, Box>(box1, box1_tf, aabb1);
computeBV<AABB, Box>(box2, box2_tf, aabb2);
aabb1.overlap(aabb2, overlap_part);
- cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()));
+ cresult->addCostSource(CostSource(overlap_part, root1->getOccupancy() * root2->getOccupancy()), crequest->num_max_cost_sources);
}
return false;
@@ -827,7 +834,7 @@ private:
-
+/// @brief Traversal node for octree collision
template<typename NarrowPhaseSolver>
class OcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
@@ -858,7 +865,7 @@ public:
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
-
+/// @brief Traversal node for octree distance
template<typename NarrowPhaseSolver>
class OcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
@@ -888,6 +895,7 @@ public:
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
+/// @brief Traversal node for shape-octree collision
template<typename S, typename NarrowPhaseSolver>
class ShapeOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
@@ -918,6 +926,7 @@ public:
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
+/// @brief Traversal node for octree-shape collision
template<typename S, typename NarrowPhaseSolver>
class OcTreeShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
@@ -948,6 +957,7 @@ public:
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
+/// @brief Traversal node for shape-octree distance
template<typename S, typename NarrowPhaseSolver>
class ShapeOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
@@ -976,6 +986,7 @@ public:
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
+/// @brief Traversal node for octree-shape distance
template<typename S, typename NarrowPhaseSolver>
class OcTreeShapeDistanceTraversalNode : public DistanceTraversalNodeBase
{
@@ -1004,7 +1015,7 @@ public:
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
-
+/// @brief Traversal node for mesh-octree collision
template<typename BV, typename NarrowPhaseSolver>
class MeshOcTreeCollisionTraversalNode : public CollisionTraversalNodeBase
{
@@ -1035,6 +1046,7 @@ public:
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
+/// @brief Traversal node for octree-mesh collision
template<typename BV, typename NarrowPhaseSolver>
class OcTreeMeshCollisionTraversalNode : public CollisionTraversalNodeBase
{
@@ -1065,7 +1077,7 @@ public:
const OcTreeSolver<NarrowPhaseSolver>* otsolver;
};
-
+/// @brief Traversal node for mesh-octree distance
template<typename BV, typename NarrowPhaseSolver>
class MeshOcTreeDistanceTraversalNode : public DistanceTraversalNodeBase
{
@@ -1095,6 +1107,7 @@ public:
};
+/// @brief Traversal node for octree-mesh distance
template<typename BV, typename NarrowPhaseSolver>
class OcTreeMeshDistanceTraversalNode : public DistanceTraversalNodeBase
{
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/traversal_node_setup.h
similarity index 75%
rename from trunk/fcl/include/fcl/simple_setup.h
rename to trunk/fcl/include/fcl/traversal_node_setup.h
index e926b672..cd29c379 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/traversal_node_setup.h
@@ -35,8 +35,8 @@
/** \author Jia Pan */
-#ifndef FCL_SIMPLE_SETUP_H
-#define FCL_SIMPLE_SETUP_H
+#ifndef FCL_TRAVERSAL_NODE_SETUP_H
+#define FCL_TRAVERSAL_NODE_SETUP_H
#include "fcl/traversal_node_bvhs.h"
#include "fcl/traversal_node_shapes.h"
@@ -44,10 +44,9 @@
#include "fcl/traversal_node_octree.h"
#include "fcl/BVH_utility.h"
-/** \brief Main namespace */
namespace fcl
{
-
+/// @brief Initialize traversal node for collision between two octrees, given current object transform
template<typename NarrowPhaseSolver>
bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
const OcTree& model1, const Transform3f& tf1,
@@ -58,7 +57,6 @@ bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -71,6 +69,7 @@ bool initialize(OcTreeCollisionTraversalNode<NarrowPhaseSolver>& node,
return true;
}
+/// @brief Initialize traversal node for distance between two octrees, given current object transform
template<typename NarrowPhaseSolver>
bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
const OcTree& model1, const Transform3f& tf1,
@@ -81,7 +80,6 @@ bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -94,6 +92,7 @@ bool initialize(OcTreeDistanceTraversalNode<NarrowPhaseSolver>& node,
return true;
}
+/// @brief Initialize traversal node for collision between one shape and one octree, given current object transform
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -104,7 +103,6 @@ bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -117,6 +115,7 @@ bool initialize(ShapeOcTreeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
return true;
}
+/// @brief Initialize traversal node for collision between one octree and one shape, given current object transform
template<typename S, typename NarrowPhaseSolver>
bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
const OcTree& model1, const Transform3f& tf1,
@@ -127,7 +126,6 @@ bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -140,6 +138,7 @@ bool initialize(OcTreeShapeCollisionTraversalNode<S, NarrowPhaseSolver>& node,
return true;
}
+/// @brief Initialize traversal node for distance between one shape and one octree, given current object transform
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -150,7 +149,6 @@ bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -163,7 +161,7 @@ bool initialize(ShapeOcTreeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
return true;
}
-
+/// @brief Initialize traversal node for distance between one octree and one shape, given current object transform
template<typename S, typename NarrowPhaseSolver>
bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
const OcTree& model1, const Transform3f& tf1,
@@ -174,7 +172,6 @@ bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -187,6 +184,7 @@ bool initialize(OcTreeShapeDistanceTraversalNode<S, NarrowPhaseSolver>& node,
return true;
}
+/// @brief Initialize traversal node for collision between one mesh and one octree, given current object transform
template<typename BV, typename NarrowPhaseSolver>
bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
@@ -197,7 +195,6 @@ bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -210,6 +207,7 @@ bool initialize(MeshOcTreeCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
return true;
}
+/// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform
template<typename BV, typename NarrowPhaseSolver>
bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
const OcTree& model1, const Transform3f& tf1,
@@ -220,7 +218,6 @@ bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -233,6 +230,7 @@ bool initialize(OcTreeMeshCollisionTraversalNode<BV, NarrowPhaseSolver>& node,
return true;
}
+/// @brief Initialize traversal node for distance between one mesh and one octree, given current object transform
template<typename BV, typename NarrowPhaseSolver>
bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
@@ -243,7 +241,6 @@ bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -256,7 +253,7 @@ bool initialize(MeshOcTreeDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
return true;
}
-
+/// @brief Initialize traversal node for collision between one octree and one mesh, given current object transform
template<typename BV, typename NarrowPhaseSolver>
bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
const OcTree& model1, const Transform3f& tf1,
@@ -267,7 +264,6 @@ bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
{
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.model2 = &model2;
@@ -281,8 +277,7 @@ bool initialize(OcTreeMeshDistanceTraversalNode<BV, NarrowPhaseSolver>& node,
}
-
-/** \brief Initialize traversal node for collision between two geometric shapes */
+/// @brief Initialize traversal node for collision between two geometric shapes, given current object transform
template<typename S1, typename S2, typename NarrowPhaseSolver>
bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
const S1& shape1, const Transform3f& tf1,
@@ -299,14 +294,13 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
node.request = request;
node.result = &result;
- result.setRequest(request);
node.cost_density = shape1.cost_density * shape2.cost_density;
return true;
}
-/** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
+/// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform
template<typename BV, typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
BVHModel<BV>& model1, Transform3f& tf1,
@@ -349,7 +343,6 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
node.request = request;
node.result = &result;
- result.setRequest(request);
node.cost_density = model1.cost_density * model2.cost_density;
@@ -357,7 +350,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
}
-/** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
+/// @brief Initialize traversal node for collision between one mesh and one shape, given current object transform
template<typename S, typename BV, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -400,14 +393,13 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
node.request = request;
node.result = &result;
- result.setRequest(request);
node.cost_density = model1.cost_density * model2.cost_density;
return true;
}
-
+/// @cond IGNORE
namespace details
{
@@ -435,7 +427,6 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
node.request = request;
node.result = &result;
- result.setRequest(request);
node.cost_density = model1.cost_density * model2.cost_density;
@@ -443,10 +434,11 @@ static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPha
}
}
+/// @endcond
-/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
+/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
const BVHModel<OBB>& model1, const Transform3f& tf1,
@@ -458,7 +450,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
-/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type */
+/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
@@ -470,7 +462,7 @@ bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
-/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type */
+/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
@@ -482,7 +474,7 @@ bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
-/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type */
+/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
@@ -495,7 +487,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
}
-
+/// @cond IGNORE
namespace details
{
template<typename S, typename BV, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
@@ -522,16 +514,15 @@ static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPha
node.request = request;
node.result = &result;
- result.setRequest(request);
node.cost_density = model1.cost_density * model2.cost_density;
return true;
}
}
+/// @endcond
-
-/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
+/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -543,7 +534,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
-/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type */
+/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -555,7 +546,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
-/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type */
+/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -567,7 +558,7 @@ bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
return details::setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
-/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type */
+/// @brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -582,7 +573,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
-/** \brief Initialize traversal node for collision between two meshes, given the current transforms */
+/// @brief Initialize traversal node for collision between two meshes, given the current transforms
template<typename BV>
bool initialize(MeshCollisionTraversalNode<BV>& node,
BVHModel<BV>& model1, Transform3f& tf1,
@@ -641,7 +632,6 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
node.request = request;
node.result = &result;
- result.setRequest(request);
node.cost_density = model1.cost_density * model2.cost_density;
@@ -649,213 +639,32 @@ bool initialize(MeshCollisionTraversalNode<BV>& node,
}
-/** \brief Initialize traversal node for collision between two meshes, specialized for OBB type */
+/// @brief Initialize traversal node for collision between two meshes, specialized for OBB type
bool initialize(MeshCollisionTraversalNodeOBB& node,
const BVHModel<OBB>& model1, const Transform3f& tf1,
const BVHModel<OBB>& model2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result);
+/// @brief Initialize traversal node for collision between two meshes, specialized for RSS type
bool initialize(MeshCollisionTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const BVHModel<RSS>& model2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result);
+/// @brief Initialize traversal node for collision between two meshes, specialized for OBBRSS type
bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result);
+/// @brief Initialize traversal node for collision between two meshes, specialized for kIOS type
bool initialize(MeshCollisionTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
const BVHModel<kIOS>& model2, const Transform3f& tf2,
const CollisionRequest& request, CollisionResult& result);
-#if USE_SVMLIGHT
-
-/** \brief Initialize traversal node for collision between two point clouds, given current transforms */
-template<typename BV, bool use_refit, bool refit_bottomup>
-bool initialize(PointCloudCollisionTraversalNode<BV>& node,
- BVHModel<BV>& model1, Transform3f& tf1,
- BVHModel<BV>& model2, Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- FCL_REAL expand_r = 1)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
- || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
- return false;
- if(!tf1.isIdentity())
- {
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = tf1.transform(p);
- vertices_transformed1[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed1);
- model1.endReplaceModel(use_refit, refit_bottomup);
-
- tf1.setIdentity();
- }
-
- if(!tf2.isIdentity())
- {
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- for(int i = 0; i < model2.num_vertices; ++i)
- {
- Vec3f& p = model2.vertices[i];
- Vec3f new_v = tf2.transform(p);
- vertices_transformed2[i] = new_v;
- }
-
- model2.beginReplaceModel();
- model2.replaceSubModel(vertices_transformed2);
- model2.endReplaceModel(use_refit, refit_bottomup);
-
- tf2.setIdentity();
- }
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.uc1.reset(new Variance3f[model1.num_vertices]);
- node.uc2.reset(new Variance3f[model2.num_vertices]);
-
- estimateSamplingVariance(model1.vertices, model1.num_vertices, node.uc1.get());
- estimateSamplingVariance(model2.vertices, model2.num_vertices, node.uc2.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
- BVHExpand(model2, node.uc2.get(), expand_r);
-
- node.request = request;
-
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- return true;
-}
-
-/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for OBB type */
-bool initialize(PointCloudCollisionTraversalNodeOBB& node,
- BVHModel<OBB>& model1, const Transform3f& tf1,
- BVHModel<OBB>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- FCL_REAL expand_r = 1);
-
-/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for RSS type */
-bool initialize(PointCloudCollisionTraversalNodeRSS& node,
- BVHModel<RSS>& model1, const Transform3f& tf1,
- BVHModel<RSS>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- FCL_REAL expand_r = 1);
-
-/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms */
-template<typename BV, bool use_refit, bool refit_bottomup>
-bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
- BVHModel<BV>& model1, Transform3f& tf1,
- BVHModel<BV>& model2, Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- FCL_REAL expand_r = 1)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- if(!tf1.isIdentity())
- {
- std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
- for(int i = 0; i < model1.num_vertices; ++i)
- {
- Vec3f& p = model1.vertices[i];
- Vec3f new_v = tf1.transform(p);
- vertices_transformed1[i] = new_v;
- }
-
- model1.beginReplaceModel();
- model1.replaceSubModel(vertices_transformed1);
- model1.endReplaceModel(use_refit, refit_bottomup);
-
- tf1.setIdentity();
- }
-
- if(!tf2.isIdentity())
- {
- std::vector<Vec3f> vertices_transformed2(model2.num_vertices);
- for(int i = 0; i < model2.num_vertices; ++i)
- {
- Vec3f& p = model2.vertices[i];
- Vec3f new_v = tf2.transform(p);
- vertices_transformed2[i] = new_v;
- }
-
- model2.beginReplaceModel();
- model2.replaceSubModel(vertices_transformed2);
- model2.endReplaceModel(use_refit, refit_bottomup);
-
- tf2.setIdentity();
- }
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices2 = model2.tri_indices;
- node.uc1.reset(new Variance3f[model1.num_vertices]);
-
- estimateSamplingVariance(model1.vertices, model1.num_vertices, node.uc1.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
-
- node.request = request;
-
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- return true;
-}
-
-
-/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for OBB type */
-bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
- BVHModel<OBB>& model1, const Transform3f& tf1,
- const BVHModel<OBB>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- FCL_REAL expand_r = 1);
-
-/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for RSS type */
-bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
- BVHModel<RSS>& model1, const Transform3f& tf1,
- const BVHModel<RSS>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold = 0.5,
- int leaf_size_threshold = 1,
- FCL_REAL expand_r = 1);
-
-#endif
-
-
-/** \brief Initialize traversal node for distance between two geometric shapes */
+/// @brief Initialize traversal node for distance between two geometric shapes
template<typename S1, typename S2, typename NarrowPhaseSolver>
bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
const S1& shape1, const Transform3f& tf1,
@@ -876,7 +685,7 @@ bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
return true;
}
-/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
+/// @brief Initialize traversal node for distance computation between two meshes, given the current transforms
template<typename BV>
bool initialize(MeshDistanceTraversalNode<BV>& node,
BVHModel<BV>& model1, Transform3f& tf1,
@@ -924,7 +733,6 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
@@ -941,27 +749,28 @@ bool initialize(MeshDistanceTraversalNode<BV>& node,
}
-/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
+/// @brief Initialize traversal node for distance computation between two meshes, specialized for RSS type
bool initialize(MeshDistanceTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
const BVHModel<RSS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result);
-
+/// @brief Initialize traversal node for distance computation between two meshes, specialized for kIOS type
bool initialize(MeshDistanceTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
const BVHModel<kIOS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result);
+/// @brief Initialize traversal node for distance computation between two meshes, specialized for OBBRSS type
bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
const DistanceRequest& request,
DistanceResult& result);
-
+/// @brief Initialize traversal node for distance computation between one mesh and one shape, given the current transforms
template<typename BV, typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
BVHModel<BV>& model1, Transform3f& tf1,
@@ -993,7 +802,6 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
@@ -1009,7 +817,7 @@ bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
return true;
}
-
+/// @brief Initialize traversal node for distance computation between one shape and one mesh, given the current transforms
template<typename S, typename BV, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -1041,7 +849,6 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
@@ -1057,7 +864,7 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
return true;
}
-
+/// @cond IGNORE
namespace details
{
@@ -1074,7 +881,6 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhas
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
@@ -1090,8 +896,9 @@ static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhas
return true;
}
}
+/// @endcond
-
+/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for RSS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const BVHModel<RSS>& model1, const Transform3f& tf1,
@@ -1103,6 +910,7 @@ bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
+/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for kIOS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const BVHModel<kIOS>& model1, const Transform3f& tf1,
@@ -1114,6 +922,7 @@ bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
+/// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
@@ -1141,7 +950,6 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhas
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
@@ -1161,7 +969,7 @@ static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhas
}
-
+/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for RSS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -1173,6 +981,7 @@ bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
+/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for kIOS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -1184,6 +993,7 @@ bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver, request, result);
}
+/// @brief Initialize traversal node for distance computation between one shape and one mesh, specialized for OBBRSS type
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
const S& model1, const Transform3f& tf1,
@@ -1197,8 +1007,7 @@ bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node
-
-/** \brief Initialize traversal node for continuous collision detection between two meshes */
+/// @brief Initialize traversal node for continuous collision detection between two meshes
template<typename BV>
bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
const BVHModel<BV>& model1, const Transform3f& tf1,
@@ -1227,64 +1036,7 @@ bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
return true;
}
-/** \brief Initialize traversal node for continuous collision detection between one mesh and one point cloud */
-template<typename BV>
-bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node,
- const BVHModel<BV>& model1, const Transform3f& tf1,
- const BVHModel<BV>& model2, const Transform3f& tf2,
- const CollisionRequest& request)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.prev_vertices1 = model1.prev_vertices;
- node.prev_vertices2 = model2.prev_vertices;
-
- node.request = request;
-
- return true;
-}
-
-/** \brief Initialize traversal node for continuous collision detection between one point cloud and one mesh */
-template<typename BV>
-bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node,
- const BVHModel<BV>& model1, const Transform3f& tf1,
- const BVHModel<BV>& model2, const Transform3f& tf2,
- const CollisionRequest& request)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices2 = model2.tri_indices;
- node.prev_vertices1 = model1.prev_vertices;
- node.prev_vertices2 = model2.prev_vertices;
-
- node.request = request;
-
- return true;
-}
-
-
-
-
-/** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms */
+/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms
template<typename BV>
bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
BVHModel<BV>& model1,
@@ -1335,9 +1087,9 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
}
-/** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS */
+/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS
inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1)
+ const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index 2784ffdc..0ea0845a 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -46,6 +46,8 @@
namespace fcl
{
+
+/// @brief Traversal node for collision between two shapes
template<typename S1, typename S2, typename NarrowPhaseSolver>
class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
@@ -58,11 +60,13 @@ public:
nsolver = NULL;
}
+ /// @brief BV culling test in one BVTT node
bool BVTesting(int, int) const
{
return false;
}
+ /// @brief Intersection testing between leaves (two shapes)
void leafTesting(int, int) const
{
if(model1->isOccupied() && model2->isOccupied())
@@ -96,7 +100,7 @@ public:
computeBV<AABB, S2>(*model2, tf2, aabb2);
AABB overlap_part;
aabb1.overlap(aabb2, overlap_part);
- result->addCostSource(CostSource(overlap_part, cost_density));
+ result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
else if((!model1->isFree() && !model2->isFree()) && request.enable_cost)
@@ -108,7 +112,7 @@ public:
computeBV<AABB, S2>(*model2, tf2, aabb2);
AABB overlap_part;
aabb1.overlap(aabb2, overlap_part);
- result->addCostSource(CostSource(overlap_part, cost_density));
+ result->addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
}
@@ -121,6 +125,7 @@ public:
const NarrowPhaseSolver* nsolver;
};
+/// @brief Traversal node for distance between two shapes
template<typename S1, typename S2, typename NarrowPhaseSolver>
class ShapeDistanceTraversalNode : public DistanceTraversalNodeBase
{
@@ -133,11 +138,13 @@ public:
nsolver = NULL;
}
+ /// @brief BV culling test in one BVTT node
FCL_REAL BVTesting(int, int) const
{
return -1; // should not be used
}
+ /// @brief Distance testing between leaves (two shapes)
void leafTesting(int, int) const
{
FCL_REAL distance;
diff --git a/trunk/fcl/include/fcl/traversal_recurse.h b/trunk/fcl/include/fcl/traversal_recurse.h
index c28fcbad..ef8b5919 100644
--- a/trunk/fcl/include/fcl/traversal_recurse.h
+++ b/trunk/fcl/include/fcl/traversal_recurse.h
@@ -46,21 +46,25 @@
namespace fcl
{
-
+/// @brief Recurse function for collision
void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
+/// @brief Recurse function for collision, specialized for OBB type
void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list);
+
+/// @brief Recurse function for collision, specialized for RSS type
void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list);
-/** Recurse function for self collision
- * Make sure node is set correctly so that the first and second tree are the same
- */
+/// @brief Recurse function for self collision. Make sure node is set correctly so that the first and second tree are the same
void selfCollisionRecurse(CollisionTraversalNodeBase* node, int b, BVHFrontList* front_list);
+/// @brief Recurse function for distance
void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
+/// @brief Recurse function for distance, using queue acceleration
void distanceQueueRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list, int qsize);
+/// @brief Recurse function for front list propagation
void propagateBVHFrontListCollisionRecurse(CollisionTraversalNodeBase* node, BVHFrontList* front_list);
diff --git a/trunk/fcl/manifest.xml b/trunk/fcl/manifest.xml
index 0ad5ac31..87ce8f46 100644
--- a/trunk/fcl/manifest.xml
+++ b/trunk/fcl/manifest.xml
@@ -10,7 +10,6 @@
<url>http://ros.org/wiki/fcl</url>
<depend package="common_rosdeps"/>
- <rosdep name="flann" />
<rosdep name="libccd" />
<rosdep name="octomap" />
diff --git a/trunk/fcl/src/BV/OBB.cpp b/trunk/fcl/src/BV/OBB.cpp
index 4de8f8d0..6df3e995 100644
--- a/trunk/fcl/src/BV/OBB.cpp
+++ b/trunk/fcl/src/BV/OBB.cpp
@@ -36,7 +36,7 @@
#include "fcl/BV/OBB.h"
#include "fcl/BVH_utility.h"
-#include "fcl/transform.h"
+#include "fcl/math/transform.h"
#include <iostream>
#include <limits>
diff --git a/trunk/fcl/src/BV/kIOS.cpp b/trunk/fcl/src/BV/kIOS.cpp
index 30171aff..3960ad5c 100644
--- a/trunk/fcl/src/BV/kIOS.cpp
+++ b/trunk/fcl/src/BV/kIOS.cpp
@@ -36,7 +36,7 @@
#include "fcl/BV/kIOS.h"
#include "fcl/BVH_utility.h"
-#include "fcl/transform.h"
+#include "fcl/math/transform.h"
#include <iostream>
#include <limits>
diff --git a/trunk/fcl/src/BVH_utility.cpp b/trunk/fcl/src/BVH_utility.cpp
index 43b362f7..12ab357f 100644
--- a/trunk/fcl/src/BVH_utility.cpp
+++ b/trunk/fcl/src/BVH_utility.cpp
@@ -36,9 +36,6 @@
#include "fcl/BVH_utility.h"
-//#include <ANN/ANN.h>
-#include <flann/flann.hpp>
-
namespace fcl
{
@@ -106,94 +103,6 @@ void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
}
-void estimateSamplingVariance(Vec3f* vertices, int num_vertices, Variance3f* ucs)
-{
- int nPts = num_vertices;
-
- int knn_k = 10;
- if(knn_k > nPts) knn_k = nPts;
- int dim = 3;
-
- float* dataset_ = new float[dim * nPts];
- float* query_ = new float[dim * 1];
-
- for(int i = 0; i < nPts; ++i)
- {
- for(int j = 0; j < dim; ++j)
- dataset_[i * dim + j] = vertices[i][j];
- }
-
-
- flann::Matrix<float> dataset(dataset_, nPts, dim);
- flann::Matrix<float> query(query_, 1, dim);
-
- int* indices_ = new int[query.rows * knn_k];
- float* dists_ = new float[query.rows * knn_k];
-
- flann::Matrix<int> indices(indices_, query.rows, knn_k);
- flann::Matrix<float> dists(dists_, query.rows, knn_k);
-
- flann::Index<flann::L2<float> > index(dataset, flann::KDTreeIndexParams(4));
- index.buildIndex();
-
- double eps = 0;
- double scale = 2;
-
- for(int i = 0; i < nPts; ++i)
- {
- float C[3][3];
- for(int j = 0; j < dim; ++j)
- {
- for(int k = 0; k < dim; ++k)
- C[j][k] = 0;
- }
-
- for(int j = 0; j < dim; ++j)
- query_[j] = vertices[i][j];
-
- index.knnSearch(query, indices, dists, knn_k, flann::SearchParams(128));
-
- double r = dists_[knn_k - 1];
- double sigma = scale * r;
-
- double weight_sum = 0;
-
- for(int j = 1; j < knn_k; ++j)
- {
- int id = indices_[j];
- Vec3f p = vertices[i] - vertices[id];
- double norm2p = p.sqrLength();
- double weight = exp(-norm2p / (sigma * sigma));
-
- weight_sum += weight;
-
- for(int k = 0; k < dim; ++k)
- {
- for(int l = 0; l < dim; ++l)
- C[k][l] += p[k] * p[l] * weight;
- }
- }
-
- for(int j = 0; j < dim; ++j)
- {
- for(int k = 0; k < dim; ++k)
- {
- C[j][k] /= weight_sum;
- ucs[i].Sigma(j, k) = C[j][k];
- }
- }
-
- ucs[i].init();
- ucs[i].sqrt();
- }
-
-
- delete [] dataset.ptr();
- delete [] query.ptr();
- delete [] indices.ptr();
- delete [] dists.ptr();
-}
-
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M)
{
Vec3f S1;
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/ccd/conservative_advancement.cpp
similarity index 97%
rename from trunk/fcl/src/conservative_advancement.cpp
rename to trunk/fcl/src/ccd/conservative_advancement.cpp
index b9b1ec1b..0eb67a29 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/ccd/conservative_advancement.cpp
@@ -34,12 +34,11 @@
/** \author Jia Pan */
-#include "fcl/conservative_advancement.h"
-
+#include "fcl/ccd/conservative_advancement.h"
+#include "fcl/ccd/motion.h"
#include "fcl/collision_node.h"
#include "fcl/traversal_node_bvhs.h"
-#include "fcl/simple_setup.h"
-#include "fcl/motion.h"
+#include "fcl/traversal_node_setup.h"
#include "fcl/collision.h"
#include "fcl/traversal_recurse.h"
diff --git a/trunk/fcl/src/motion.cpp b/trunk/fcl/src/ccd/motion.cpp
similarity index 99%
rename from trunk/fcl/src/motion.cpp
rename to trunk/fcl/src/ccd/motion.cpp
index af50a3cd..ad84a1de 100644
--- a/trunk/fcl/src/motion.cpp
+++ b/trunk/fcl/src/ccd/motion.cpp
@@ -35,7 +35,7 @@
/** \author Jia Pan */
-#include "fcl/motion.h"
+#include "fcl/ccd/motion.h"
namespace fcl
{
diff --git a/trunk/fcl/src/collision_data.cpp b/trunk/fcl/src/collision_data.cpp
new file mode 100644
index 00000000..1d7a0bc1
--- /dev/null
+++ b/trunk/fcl/src/collision_data.cpp
@@ -0,0 +1,52 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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 Jia Pan */
+
+#include "fcl/collision_data.h"
+
+namespace fcl
+{
+
+bool CollisionRequest::isSatisfied(const CollisionResult& result) const
+{
+ return (!enable_cost) && result.isCollision() && (num_max_contacts <= result.numContacts());
+}
+
+bool DistanceRequest::isSatisfied(const DistanceResult& result) const
+{
+ return (result.min_distance <= 0);
+}
+
+}
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index b3d343bc..48b3ae56 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -37,7 +37,7 @@
#include "fcl/collision_func_matrix.h"
-#include "fcl/simple_setup.h"
+#include "fcl/traversal_node_setup.h"
#include "fcl/collision_node.h"
#include "fcl/narrowphase/narrowphase.h"
@@ -50,7 +50,7 @@ std::size_t ShapeOcTreeCollide(const CollisionGeometry* o1, const Transform3f& t
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
ShapeOcTreeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
const T_SH* obj1 = static_cast<const T_SH*>(o1);
@@ -68,7 +68,7 @@ std::size_t OcTreeShapeCollide(const CollisionGeometry* o1, const Transform3f& t
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
OcTreeShapeCollisionTraversalNode<T_SH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
@@ -86,7 +86,7 @@ std::size_t OcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, c
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
OcTreeCollisionTraversalNode<NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
@@ -104,7 +104,7 @@ std::size_t OcTreeBVHCollide(const CollisionGeometry* o1, const Transform3f& tf1
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
OcTreeMeshCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
@@ -122,7 +122,7 @@ std::size_t BVHOcTreeCollide(const CollisionGeometry* o1, const Transform3f& tf1
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshOcTreeCollisionTraversalNode<T_BVH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
@@ -141,7 +141,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
@@ -160,7 +160,7 @@ struct BVHShapeCollider
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
@@ -184,7 +184,7 @@ struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshShapeCollisionTraversalNodeOBB<T_SH, NarrowPhaseSolver> node;
const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
@@ -205,7 +205,7 @@ struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshShapeCollisionTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
@@ -226,7 +226,7 @@ struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshShapeCollisionTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
@@ -247,7 +247,7 @@ struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshShapeCollisionTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
@@ -264,7 +264,7 @@ struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
template<typename T_BVH>
std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshCollisionTraversalNode<T_BVH> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
@@ -286,7 +286,7 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, cons
template<>
std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshCollisionTraversalNodeOBB node;
const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
@@ -301,7 +301,7 @@ std::size_t BVHCollide<OBB>(const CollisionGeometry* o1, const Transform3f& tf1,
template<>
std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshCollisionTraversalNodeOBBRSS node;
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
@@ -317,7 +317,7 @@ std::size_t BVHCollide<OBBRSS>(const CollisionGeometry* o1, const Transform3f& t
template<>
std::size_t BVHCollide<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const CollisionRequest& request, CollisionResult& result)
{
- if(!request.enable_cost && (request.num_max_contacts <= result.numContacts())) return result.numContacts();
+ if(request.isSatisfied(result)) return result.numContacts();
MeshCollisionTraversalNodekIOS node;
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index a2af52e6..d09f82e3 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -37,7 +37,7 @@
#include "fcl/distance_func_matrix.h"
#include "fcl/collision_node.h"
-#include "fcl/simple_setup.h"
+#include "fcl/traversal_node_setup.h"
#include "fcl/narrowphase/narrowphase.h"
namespace fcl
@@ -47,6 +47,7 @@ template<typename T_SH, typename NarrowPhaseSolver>
FCL_REAL ShapeOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
ShapeOcTreeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
const T_SH* obj1 = static_cast<const T_SH*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
@@ -62,6 +63,7 @@ template<typename T_SH, typename NarrowPhaseSolver>
FCL_REAL OcTreeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
OcTreeShapeDistanceTraversalNode<T_SH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
@@ -77,6 +79,7 @@ template<typename NarrowPhaseSolver>
FCL_REAL OcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
OcTreeDistanceTraversalNode<NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
@@ -92,6 +95,7 @@ template<typename T_BVH, typename NarrowPhaseSolver>
FCL_REAL BVHOcTreeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
MeshOcTreeDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
const OcTree* obj2 = static_cast<const OcTree*>(o2);
@@ -107,6 +111,7 @@ template<typename T_BVH, typename NarrowPhaseSolver>
FCL_REAL OcTreeBVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
OcTreeMeshDistanceTraversalNode<T_BVH, NarrowPhaseSolver> node;
const OcTree* obj1 = static_cast<const OcTree*>(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
@@ -124,6 +129,7 @@ template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
ShapeDistanceTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
@@ -140,6 +146,7 @@ struct BVHShapeDistancer
static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
MeshShapeDistanceTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
@@ -160,6 +167,7 @@ struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
MeshShapeDistanceTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
@@ -178,6 +186,7 @@ struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
MeshShapeDistanceTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
@@ -195,6 +204,7 @@ struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
static FCL_REAL distance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
MeshShapeDistanceTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
@@ -211,6 +221,7 @@ template<typename T_BVH>
FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
MeshDistanceTraversalNode<T_BVH> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
@@ -229,6 +240,7 @@ template<>
FCL_REAL BVHDistance<RSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
MeshDistanceTraversalNodeRSS node;
const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
const BVHModel<RSS>* obj2 = static_cast<const BVHModel<RSS>* >(o2);
@@ -243,6 +255,7 @@ template<>
FCL_REAL BVHDistance<kIOS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
MeshDistanceTraversalNodekIOS node;
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
@@ -258,6 +271,7 @@ template<>
FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
const DistanceRequest& request, DistanceResult& result)
{
+ if(request.isSatisfied(result)) return result.min_distance;
MeshDistanceTraversalNodeOBBRSS node;
const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
const BVHModel<OBBRSS>* obj2 = static_cast<const BVHModel<OBBRSS>* >(o2);
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 46a10dc3..70975242 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -166,119 +166,11 @@ int PolySolver::solveCubic(FCL_REAL c[4], FCL_REAL s[3])
// re-substitute
sub = ONE_OVER_THREE * A;
for(i = 0; i < num; i++)
- s[i] -= sub;
+ s[i] -= sub;
return num;
}
-#if USE_SVMLIGHT
-CloudClassifierParam::CloudClassifierParam()
-{
- strcpy(learn_parm.predfile, "");
- strcpy(learn_parm.alphafile, "");
- learn_parm.biased_hyperplane = 1;
- learn_parm.sharedslack = 0;
- learn_parm.remove_inconsistent = 0;
- learn_parm.skip_final_opt_check = 0;
- learn_parm.svm_maxqpsize = 10;
- learn_parm.svm_newvarsinqp = 0;
- learn_parm.svm_iter_to_shrink = -9999;
- learn_parm.maxiter = 100000;
- learn_parm.kernel_cache_size = 40;
- learn_parm.svm_c = 10;
- learn_parm.eps = 0.1;
- learn_parm.transduction_posratio = -1.0;
- learn_parm.svm_costratio = 1.0;
- learn_parm.svm_costratio_unlab = 1.0;
- learn_parm.svm_unlabbound = 1E-5;
- learn_parm.epsilon_crit = 0.001;
- learn_parm.epsilon_a = 1E-15;
- learn_parm.compute_loo = 0;
- learn_parm.rho = 1.0;
- learn_parm.xa_depth = 0;
- learn_parm.type = CLASSIFICATION;
- kernel_parm.kernel_type = 2;
- kernel_parm.poly_degree = 3;
- kernel_parm.rbf_gamma = 1.0;
- kernel_parm.coef_lin = 1;
- kernel_parm.coef_const = 1;
- strcpy(kernel_parm.custom, "empty");
-
- bool res = true;
-
- if(learn_parm.svm_iter_to_shrink == -9999)
- {
- if(kernel_parm.kernel_type == LINEAR)
- learn_parm.svm_iter_to_shrink = 2;
- else
- learn_parm.svm_iter_to_shrink = 100;
- }
-
- if((learn_parm.skip_final_opt_check)
- && (kernel_parm.kernel_type == LINEAR))
- {
- std::cout << "It does not make sense to skip the final optimality check for linear kernels." << std::endl;
- learn_parm.skip_final_opt_check = 0;
- }
- if((learn_parm.skip_final_opt_check)
- && (learn_parm.remove_inconsistent))
- {
- std::cout << "It is necessary to do the final optimality check when removing inconsistent examples." << std::endl;
- res = false;
- }
- if((learn_parm.svm_maxqpsize < 2))
- {
- std::cout << "Maximum size of QP-subproblems not in valid range: " << learn_parm.svm_maxqpsize << "[2..]" << std::endl;
- res = false;
- }
- if((learn_parm.svm_maxqpsize < learn_parm.svm_newvarsinqp))
- {
- std::cout << "Maximum size of QP-subproblems " << learn_parm.svm_maxqpsize << " must be larger than the number of new variables [" << learn_parm.svm_newvarsinqp << "] entering the working set in each iteration." << std::endl;
- res = false;
- }
- if(learn_parm.svm_iter_to_shrink < 1)
- {
- std::cout << "Maximum number of iterations for shrinking not in valid range: " << learn_parm.svm_iter_to_shrink << " [1,..]." << std::endl;
- res = false;
- }
- if(learn_parm.svm_c < 0)
- {
- std::cout << "The C parameter must be greater than zero!" << std::endl;
- res = false;
- }
- if(learn_parm.transduction_posratio > 1)
- {
- std::cout << "The fraction of unlabeled examples to classify as positives must be less than 1.0." << std::endl;
- res = false;
- }
- if(learn_parm.svm_costratio <= 0)
- {
- std::cout << "The COSTRATIO parameter must be greater than zero!" << std::endl;
- res = false;
- }
- if(learn_parm.epsilon_crit <= 0)
- {
- std::cout << "The epsilon parameter must be greater than zero!" << std::endl;
- res = false;
- }
- if(learn_parm.rho < 0)
- {
- std::cout << "The parameter rho for xi/alpha-estimates and leave-one-out pruning must be greater than zero (typically 1.0 or 2.0, see T. Joachims, Estimating the Generalization Performance of an SVM Efficiently, ICML, 2000.)" << std::endl;
- res = false;
- }
- if((learn_parm.xa_depth < 0) || (learn_parm.xa_depth > 100))
- {
- std::cout << "The parameter depth for ext. xi/alpha-estimates must be in [0..100] (zero for switching to the conventional xa/estimates described in T. Joachims, Estimating the Generalization Performance of an SVM Efficiently, ICML, 2000.)" << std::endl;
- res = false;
- }
-
- if(!res)
- {
- std::cout << "Solver initialization fails" << std::endl;
- }
-}
-
-#endif
const FCL_REAL Intersect::EPSILON = 1e-5;
const FCL_REAL Intersect::NEAR_ZERO_THRESHOLD = 1e-7;
@@ -290,12 +182,10 @@ bool Intersect::isZero(FCL_REAL v)
return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
}
-/** \brief
- * data: only used for EE, return the intersect point
- */
+/// @brief data: only used for EE, return the intersect point
bool Intersect::solveCubicWithIntervalNewton(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data)
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
+ FCL_REAL& l, FCL_REAL& r, bool bVF, FCL_REAL coeffs[], Vec3f* data)
{
FCL_REAL v2[2]= {l*l,r*r};
FCL_REAL v[2]= {l,r};
@@ -395,14 +285,13 @@ bool Intersect::insideLineSegment(const Vec3f& a, const Vec3f& b, const Vec3f& p
return (p - a).dot(p - b) <= 0;
}
-/* \brief Calculate the line segment papb that is the shortest route between
- two lines p1p2 and p3p4. Calculate also the values of mua and mub where
- pa = p1 + mua (p2 - p1)
- pb = p3 + mub (p4 - p3)
- Return FALSE if no solution exists.
-*/
+/// @brief Calculate the line segment papb that is the shortest route between
+/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where
+/// pa = p1 + mua (p2 - p1)
+/// pb = p3 + mub (p4 - p3)
+/// Return FALSE if no solution exists.
bool Intersect::linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& p4,
- Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub)
+ Vec3f* pa, Vec3f* pb, FCL_REAL* mua, FCL_REAL* mub)
{
Vec3f p31 = p1 - p3;
Vec3f p34 = p4 - p3;
@@ -438,15 +327,15 @@ bool Intersect::linelineIntersect(const Vec3f& p1, const Vec3f& p2, const Vec3f&
}
bool Intersect::checkRootValidity_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- FCL_REAL t)
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
+ FCL_REAL t)
{
return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t);
}
bool Intersect::checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL t, Vec3f* q_i)
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
+ FCL_REAL t, Vec3f* q_i)
{
Vec3f a = a0 + va * t;
Vec3f b = b0 + vb * t;
@@ -464,17 +353,17 @@ bool Intersect::checkRootValidity_EE(const Vec3f& a0, const Vec3f& b0, const Vec
}
bool Intersect::checkRootValidity_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- FCL_REAL t)
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
+ FCL_REAL t)
{
return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t);
}
bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- bool bVF,
- FCL_REAL* ret)
+ const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
+ bool bVF,
+ FCL_REAL* ret)
{
FCL_REAL discriminant = b * b - 4 * a * c;
if(discriminant < 0)
@@ -507,8 +396,8 @@ bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
}
bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
- const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp)
+ const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vp)
{
if(isZero(a))
{
@@ -533,12 +422,11 @@ bool Intersect::solveSquare(FCL_REAL a, FCL_REAL b, FCL_REAL c,
-/** \brief Compute the cubic coefficients for VF case
- * See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
- */
+/// @brief Compute the cubic coefficients for VF case
+/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
void Intersect::computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vp,
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
{
Vec3f vavb = vb - va;
Vec3f vavc = vc - va;
@@ -559,8 +447,8 @@ void Intersect::computeCubicCoeff_VF(const Vec3f& a0, const Vec3f& b0, const Vec
}
void Intersect::computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vc, const Vec3f& vd,
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c, FCL_REAL* d)
{
Vec3f vavb = vb - va;
Vec3f vcvd = vd - vc;
@@ -580,9 +468,9 @@ void Intersect::computeCubicCoeff_EE(const Vec3f& a0, const Vec3f& b0, const Vec
}
void Intersect::computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
- const Vec3f& L,
- FCL_REAL* a, FCL_REAL* b, FCL_REAL* c)
+ const Vec3f& va, const Vec3f& vb, const Vec3f& vp,
+ const Vec3f& L,
+ FCL_REAL* a, FCL_REAL* b, FCL_REAL* c)
{
Vec3f vbva = va - vb;
Vec3f vbvp = vp - vb;
@@ -599,8 +487,8 @@ void Intersect::computeCubicCoeff_VE(const Vec3f& a0, const Vec3f& b0, const Vec
bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
{
*collision_time = 2.0;
@@ -618,13 +506,11 @@ bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
return false;
}
- /*
- if(isZero(a))
- {
- return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time);
- }
- */
+ /// if(isZero(a))
+ /// {
+ /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time);
+ /// }
FCL_REAL coeffs[4];
coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
@@ -665,8 +551,8 @@ bool Intersect::intersect_VF(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
}
bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
{
*collision_time = 2.0;
@@ -683,12 +569,12 @@ bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
{
return false;
}
- /*
- if(isZero(a))
- {
- return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false);
- }
- */
+
+ /// if(isZero(a))
+ /// {
+ /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false);
+ /// }
+
FCL_REAL coeffs[4];
coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d;
@@ -730,8 +616,8 @@ bool Intersect::intersect_EE(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0,
bool Intersect::intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
- const Vec3f& L)
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& p1,
+ const Vec3f& L)
{
Vec3f va, vb, vp;
va = a1 - a0;
@@ -749,9 +635,9 @@ bool Intersect::intersect_VE(const Vec3f& a0, const Vec3f& b0, const Vec3f& p0,
}
-/** \brief Prefilter for intersection, works for both VF and EE */
+/// @brief Prefilter for intersection, works for both VF and EE
bool Intersect::intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1)
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1)
{
Vec3f n0 = (b0 - a0).cross(c0 - a0);
Vec3f n1 = (b1 - a1).cross(c1 - a1);
@@ -780,8 +666,8 @@ bool Intersect::intersectPreFiltering(const Vec3f& a0, const Vec3f& b0, const Ve
}
bool Intersect::intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& p0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& p1,
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
{
if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1))
{
@@ -792,8 +678,8 @@ bool Intersect::intersect_VF_filtered(const Vec3f& a0, const Vec3f& b0, const Ve
}
bool Intersect::intersect_EE_filtered(const Vec3f& a0, const Vec3f& b0, const Vec3f& c0, const Vec3f& d0,
- const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
- FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
+ const Vec3f& a1, const Vec3f& b1, const Vec3f& c1, const Vec3f& d1,
+ FCL_REAL* collision_time, Vec3f* p_i, bool useNewton)
{
if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1))
{
@@ -889,7 +775,7 @@ bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f
return false;
- /* Return contact information */
+ /// Return contact information
if(contact_points && num_contact_points && penetration_depth && normal)
{
if(penetration_depth1 > penetration_depth2)
@@ -1261,8 +1147,8 @@ bool Intersect::sameSideOfPlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& v
}
int Intersect::project6(const Vec3f& ax,
- const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
- const Vec3f& q1, const Vec3f& q2, const Vec3f& q3)
+ const Vec3f& p1, const Vec3f& p2, const Vec3f& p3,
+ const Vec3f& q1, const Vec3f& q2, const Vec3f& q3)
{
FCL_REAL P1 = ax.dot(p1);
FCL_REAL P2 = ax.dot(p2);
@@ -1283,566 +1169,6 @@ int Intersect::project6(const Vec3f& ax,
}
-#if USE_SVMLIGHT
-
-void Intersect::singleKernelGradient(KERNEL_PARM *kernel_parm, SVECTOR *a, SVECTOR *b, Vec3f& g)
-{
- double tmp;
- switch(kernel_parm->kernel_type)
- {
- case 0: /* linear */
- g[0] = a->words[0].weight;
- g[1] = a->words[1].weight;
- g[2] = a->words[2].weight;
- break;
- case 1: /* polynomial */
- tmp = pow(kernel_parm->coef_lin * sprod_ss(a, b) + kernel_parm->coef_const, (double)kernel_parm->poly_degree - 1) * kernel_parm->poly_degree * kernel_parm->coef_lin;
- g[0] = tmp * a->words[0].weight;
- g[1] = tmp * a->words[1].weight;
- g[2] = tmp * a->words[2].weight;
- break;
- case 2: /* radial basis function */
- tmp = (exp(-kernel_parm->rbf_gamma * (a->twonorm_sq - 2 * sprod_ss(a, b) + b->twonorm_sq))) * 2 * (-kernel_parm->rbf_gamma);
- g[0] = tmp * (b->words[0].weight - a->words[0].weight);
- g[1] = tmp * (b->words[1].weight - a->words[1].weight);
- g[2] = tmp * (b->words[2].weight - a->words[2].weight);
- break;
- case 3: /* sigmoid neural net */
- tmp = 1 - (tanh(kernel_parm->coef_lin * sprod_ss(a, b) + kernel_parm->coef_const)) * (tanh(kernel_parm->coef_lin * sprod_ss(a, b) + kernel_parm->coef_const));
- tmp *= kernel_parm->coef_lin;
- g[0] = tmp * a->words[0].weight;
- g[1] = tmp * a->words[1].weight;
- g[2] = tmp * a->words[2].weight;
- break;
- default:
- std::cout << "Error: Unknown kernel function" << std::endl;
- return;
- }
-}
-
-void Intersect::kernelGradient(KERNEL_PARM *kernel_parm, DOC *a, DOC *b, Vec3f& g)
-{
- g.setValue(0, 0, 0);
- SVECTOR *fa, *fb;
- Vec3f tmp;
-
- for(fa = a->fvec; fa; fa = fa->next)
- {
- for(fb = b->fvec; fb; fb = fb->next)
- {
- if (fa->kernel_id == fb->kernel_id)
- {
- singleKernelGradient(kernel_parm, fa, fb, tmp);
- g += (tmp * (fa->factor * fb->factor));
- }
- }
- }
-}
-
-FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
- Vec3f* cloud2, Variance3f* uc2, int size_cloud2,
- const CloudClassifierParam& solver, bool scaling)
-{
- KERNEL_CACHE *kernel_cache;
- LEARN_PARM learn_parm = solver.learn_parm;
- KERNEL_PARM kernel_parm = solver.kernel_parm;
- MODEL *model = (MODEL *)my_malloc(sizeof(MODEL));
-
- int nPositiveExamples = size_cloud1;
- int nNegativeExamples = size_cloud2;
- int totdoc = nPositiveExamples + nNegativeExamples;
- int totwords = 3;
-
- int queryid = 0;
- int slackid = 0;
- double costfactor = 1;
- WORD *words = (WORD *)my_malloc(sizeof(WORD) * 4);
-
- DOC** docs = (DOC **)my_malloc(sizeof(DOC *) * (nPositiveExamples + nNegativeExamples));
- double* label = (double *)my_malloc(sizeof(double) * (nPositiveExamples + nNegativeExamples));
-
- float bbmin[3] = {0, 0, 0};
- float bbmax[3] = {0, 0, 0};
-
- for(int i = 0; i < nPositiveExamples; ++i)
- {
- int example_id = i;
- label[example_id] = 1;
- float coord1[3];
- coord1[0] = cloud1[i][0];
- coord1[1] = cloud1[i][1];
- coord1[2] = cloud1[i][2];
-
-
- words[0].wnum = 1;
- words[0].weight = coord1[0];
- words[1].wnum = 2;
- words[1].weight = coord1[1];
- words[2].wnum = 3;
- words[2].weight = coord1[2];
- words[3].wnum = 0;
- words[3].weight = 3;
-
- docs[example_id] = create_example(example_id, queryid, slackid, costfactor,
- create_svector(words, (char*)"", 1.0));
-
- if(scaling)
- {
- if(i == 0)
- {
- bbmin[0] = coord1[0];
- bbmin[1] = coord1[1];
- bbmin[2] = coord1[2];
- bbmax[0] = coord1[0];
- bbmax[1] = coord1[1];
- bbmax[2] = coord1[2];
- }
- else
- {
- if(bbmin[0] > coord1[0]) bbmin[0] = coord1[0];
- if(bbmax[0] < coord1[0]) bbmax[0] = coord1[0];
- if(bbmin[1] > coord1[1]) bbmin[1] = coord1[1];
- if(bbmax[1] < coord1[1]) bbmax[1] = coord1[1];
- if(bbmin[2] > coord1[2]) bbmin[2] = coord1[2];
- if(bbmax[2] < coord1[2]) bbmax[2] = coord1[2];
- }
- }
- }
-
-
- for(int i = 0; i < nNegativeExamples; ++i)
- {
- int example_id = i + nPositiveExamples;
- label[example_id] = -1;
- float coord1[3];
-
- coord1[0] = cloud2[i][0];
- coord1[1] = cloud2[i][1];
- coord1[2] = cloud2[i][2];
-
- words[0].wnum = 1;
- words[0].weight = coord1[0];
- words[1].wnum = 2;
- words[1].weight = coord1[1];
- words[2].wnum = 3;
- words[2].weight = coord1[2];
- words[3].wnum = 0;
- words[3].weight = 3;
- docs[example_id] = create_example(example_id, queryid, slackid, costfactor,
- create_svector(words, (char*)"", 1.0));
-
- if(scaling)
- {
- if(bbmin[0] > coord1[0]) bbmin[0] = coord1[0];
- if(bbmax[0] < coord1[0]) bbmax[0] = coord1[0];
- if(bbmin[1] > coord1[1]) bbmin[1] = coord1[1];
- if(bbmax[1] < coord1[1]) bbmax[1] = coord1[1];
- if(bbmin[2] > coord1[2]) bbmin[2] = coord1[2];
- if(bbmax[2] < coord1[2]) bbmax[2] = coord1[2];
- }
- }
-
- FCL_REAL S[3];
- S[0] = 1 / (bbmax[0] - bbmin[0]);
- S[1] = 1 / (bbmax[1] - bbmin[1]);
- S[2] = 1 / (bbmax[2] - bbmin[2]);
-
- if(scaling)
- {
- for(int i = 0; i < totdoc; ++i)
- {
- FCL_REAL f = docs[i]->fvec->words[0].weight;
- docs[i]->fvec->words[0].weight = (f - bbmin[0]) * S[0];
- f = docs[i]->fvec->words[1].weight;
- docs[i]->fvec->words[1].weight = (f - bbmin[1]) * S[1];
- f = docs[i]->fvec->words[2].weight;
- docs[i]->fvec->words[2].weight = (f - bbmin[2]) * S[2];
- docs[i]->fvec->twonorm_sq = sprod_ss(docs[i]->fvec, docs[i]->fvec);
- }
- }
-
- if(kernel_parm.kernel_type == LINEAR) /** don't need the cache */
- kernel_cache = NULL;
- else
- {
- /** Always get a new kernel cache. It is not possible to use the
- * same cache for two different training runs
- */
- kernel_cache = kernel_cache_init(totdoc, learn_parm.kernel_cache_size);
- }
-
-
- double *alpha_in = NULL;
- int nerrors;
- double maxerror;
- svm_learn_classification_extend(docs, label, totdoc, totwords, &learn_parm,
- &kernel_parm, kernel_cache, model, alpha_in, &nerrors, &maxerror);
-
- /** compute collision probability */
- double max_collision_prob = 0;
- for(int i = 0; i < totdoc; ++i)
- {
- Vec3f fgrad;
- Vec3f g;
- double f = - label[i] * classify_example(model, docs[i]);
-
- for(int j = 1; j < model->sv_num; j++)
- {
- kernelGradient(&model->kernel_parm, model->supvec[j], docs[i], g);
- fgrad += (g * (model->alpha[j] * label[j]));
-
- if (i < nPositiveExamples)
- {
- double sigma = quadraticForm(uc1[i].Sigma, fgrad);
- FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
- if(max_collision_prob < col_prob)
- max_collision_prob = col_prob;
- }
- else
- {
- double sigma = uc2[i - quadraticForm(nPositiveExamples].Sigma, fgrad);
- FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
- if(max_collision_prob < col_prob)
- max_collision_prob = col_prob;
- }
- }
- }
-
- if(kernel_cache)
- {
- kernel_cache_cleanup(kernel_cache);
- kernel_cache = NULL;
- }
-
-
- free(alpha_in);
- free_model(model, 0);
- for (int i = 0; i < totdoc; i++)
- free_example(docs[i], 1);
- free(docs);
- free(label);
- free(words);
-
- return max_collision_prob;
-}
-
-FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
- Vec3f* cloud2, Variance3f* uc2, int size_cloud2,
- const Matrix3f& R, const Vec3f& T, const CloudClassifierParam& solver, bool scaling)
-{
- KERNEL_CACHE *kernel_cache;
- LEARN_PARM learn_parm = solver.learn_parm;
- KERNEL_PARM kernel_parm = solver.kernel_parm;
- MODEL *model = (MODEL *)my_malloc(sizeof(MODEL));
-
- int nPositiveExamples = size_cloud1;
- int nNegativeExamples = size_cloud2;
- int totdoc = nPositiveExamples + nNegativeExamples;
- int totwords = 3;
-
- int queryid = 0;
- int slackid = 0;
- double costfactor = 1;
- WORD *words = (WORD *)my_malloc(sizeof(WORD) * 4);
-
- DOC** docs = (DOC **)my_malloc(sizeof(DOC *) * (nPositiveExamples + nNegativeExamples));
- double* label = (double *)my_malloc(sizeof(double) * (nPositiveExamples + nNegativeExamples));
-
- float bbmin[3] = {0, 0, 0};
- float bbmax[3] = {0, 0, 0};
-
- for(int i = 0; i < nPositiveExamples; ++i)
- {
- int example_id = i;
- label[example_id] = 1;
- float coord1[3];
- coord1[0] = cloud1[i][0];
- coord1[1] = cloud1[i][1];
- coord1[2] = cloud1[i][2];
-
-
- words[0].wnum = 1;
- words[0].weight = coord1[0];
- words[1].wnum = 2;
- words[1].weight = coord1[1];
- words[2].wnum = 3;
- words[2].weight = coord1[2];
- words[3].wnum = 0;
- words[3].weight = 3;
-
- docs[example_id] = create_example(example_id, queryid, slackid, costfactor,
- create_svector(words, (char*)"", 1.0));
-
- if(scaling)
- {
- if(i == 0)
- {
- bbmin[0] = coord1[0];
- bbmin[1] = coord1[1];
- bbmin[2] = coord1[2];
- bbmax[0] = coord1[0];
- bbmax[1] = coord1[1];
- bbmax[2] = coord1[2];
- }
- else
- {
- if(bbmin[0] > coord1[0]) bbmin[0] = coord1[0];
- if(bbmax[0] < coord1[0]) bbmax[0] = coord1[0];
- if(bbmin[1] > coord1[1]) bbmin[1] = coord1[1];
- if(bbmax[1] < coord1[1]) bbmax[1] = coord1[1];
- if(bbmin[2] > coord1[2]) bbmin[2] = coord1[2];
- if(bbmax[2] < coord1[2]) bbmax[2] = coord1[2];
- }
- }
- }
-
-
- for(int i = 0; i < nNegativeExamples; ++i)
- {
- int example_id = i + nPositiveExamples;
- label[example_id] = -1;
- Vec3f coord0, coord1;
-
- coord0[0] = cloud2[i][0];
- coord0[1] = cloud2[i][1];
- coord0[2] = cloud2[i][2];
- coord1 = R * coord0 + T; // rotate the coordinate
-
- words[0].wnum = 1;
- words[0].weight = coord1[0];
- words[1].wnum = 2;
- words[1].weight = coord1[1];
- words[2].wnum = 3;
- words[2].weight = coord1[2];
- words[3].wnum = 0;
- words[3].weight = 3;
- docs[example_id] = create_example(example_id, queryid, slackid, costfactor,
- create_svector(words, (char*)"", 1.0));
-
- if(scaling)
- {
- if(bbmin[0] > coord1[0]) bbmin[0] = coord1[0];
- if(bbmax[0] < coord1[0]) bbmax[0] = coord1[0];
- if(bbmin[1] > coord1[1]) bbmin[1] = coord1[1];
- if(bbmax[1] < coord1[1]) bbmax[1] = coord1[1];
- if(bbmin[2] > coord1[2]) bbmin[2] = coord1[2];
- if(bbmax[2] < coord1[2]) bbmax[2] = coord1[2];
- }
- }
-
- FCL_REAL S[3];
- S[0] = 1 / (bbmax[0] - bbmin[0]);
- S[1] = 1 / (bbmax[1] - bbmin[1]);
- S[2] = 1 / (bbmax[2] - bbmin[2]);
-
- if(scaling)
- {
- for(int i = 0; i < totdoc; ++i)
- {
- FCL_REAL f = docs[i]->fvec->words[0].weight;
- docs[i]->fvec->words[0].weight = (f - bbmin[0]) * S[0];
- f = docs[i]->fvec->words[1].weight;
- docs[i]->fvec->words[1].weight = (f - bbmin[1]) * S[1];
- f = docs[i]->fvec->words[2].weight;
- docs[i]->fvec->words[2].weight = (f - bbmin[2]) * S[2];
- docs[i]->fvec->twonorm_sq = sprod_ss(docs[i]->fvec, docs[i]->fvec);
- }
- }
-
- if(kernel_parm.kernel_type == LINEAR) /** don't need the cache */
- kernel_cache = NULL;
- else
- {
- /** Always get a new kernel cache. It is not possible to use the
- * same cache for two different training runs
- */
- kernel_cache = kernel_cache_init(totdoc, learn_parm.kernel_cache_size);
- }
-
-
- double *alpha_in = NULL;
- int nerrors;
- double maxerror;
- svm_learn_classification_extend(docs, label, totdoc, totwords, &learn_parm,
- &kernel_parm, kernel_cache, model, alpha_in, &nerrors, &maxerror);
-
- /** compute collision probability */
- double max_collision_prob = 0;
- for(int i = 0; i < totdoc; ++i)
- {
- Vec3f fgrad;
- Vec3f g;
- double f = - label[i] * classify_example(model, docs[i]);
-
- for(int j = 1; j < model->sv_num; j++)
- {
- kernelGradient(&model->kernel_parm, model->supvec[j], docs[i], g);
- fgrad += (g * (model->alpha[j] * label[j]));
-
- if (i < nPositiveExamples)
- {
- double sigma = quadraticForm(uc1[i].Sigma, fgrad);
- FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
- if(max_collision_prob < col_prob)
- max_collision_prob = col_prob;
- }
- else
- {
- Matrix3f rotatedSigma = R.tensorTransform(uc2[i - nPositiveExamples].Sigma);
- double sigma = quadraticForm(rotatedSigma, fgrad);
- FCL_REAL col_prob = gaussianCDF(f / sqrt(sigma));
- if(max_collision_prob < col_prob)
- max_collision_prob = col_prob;
- }
- }
- }
-
- if(kernel_cache)
- {
- kernel_cache_cleanup(kernel_cache);
- kernel_cache = NULL;
- }
-
-
- free(alpha_in);
- free_model(model, 0);
- for (int i = 0; i < totdoc; i++)
- free_example(docs[i], 1);
- free(docs);
- free(label);
- free(words);
-
- return max_collision_prob;
-}
-
-
-FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3)
-{
- // get the plane x * n - t = 0 and the compute the projection matrix according to (I - nn^t) y + t * n = y'
- FCL_REAL t;
- Vec3f n;
- bool b_plane = buildTrianglePlane(Q1, Q2, Q3, &n, &t);
- if(!b_plane)
- {
- std::cerr << "build triangle plane failed!" << std::endl;
- return 0.0;
- }
-
- FCL_REAL edge_t[3];
- Vec3f edge_n[3];
-
- bool b_edge_plane1 = buildEdgePlane(Q1, Q2, n, edge_n + 0, edge_t + 0);
- if(!b_edge_plane1)
- {
- std::cerr << "build edge plane1 failed!" << std::endl;
- return 0.0;
- }
-
- bool b_edge_plane2 = buildEdgePlane(Q2, Q3, n, edge_n + 1, edge_t + 1);
- if(!b_edge_plane2)
- {
- std::cerr << "build edge plane2 failed!" << std::endl;
- return 0.0;
- }
-
- bool b_edge_plane3 = buildEdgePlane(Q3, Q1, n, edge_n + 2, edge_t + 2);
- if(!b_edge_plane3)
- {
- std::cerr << "build edge plane3 failed!" << std::endl;
- return 0.0;
- }
-
- Matrix3f P(1 - n[0] * n[0], -n[0] * n[1], -n[0] * n[2],
- -n[1] * n[0], 1 - n[1] * n[1], -n[1] * n[2],
- -n[2] * n[0], -n[2] * n[1], 1 - n[2] * n[2]);
-
- Vec3f delta = n * t;
-
- FCL_REAL max_prob = 0;
- for(int i = 0; i < size_cloud1; ++i)
- {
- Vec3f projected_p = P * cloud1[i] + delta;
-
- // compute the projected uncertainty by P * S * P'
- Matrix3f newS = P.tensorTransform(uc1[i].Sigma);
-
- // check whether the point is inside or outside the triangle
-
- bool b_inside = insideTriangle(Q1, Q2, Q3, projected_p);
-
- if(b_inside)
- {
- FCL_REAL prob1 = gaussianCDF((projected_p.dot(edge_n[0]) - edge_t[0]) / sqrt(quadraticForm(newS, edge_n[0])));
- FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[1]) - edge_t[1]) / sqrt(quadraticForm(newS, edge_n[1])));
- FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[2]) - edge_t[2]) / sqrt(quadraticForm(newS, edge_n[2])));
- FCL_REAL prob = 1.0 - prob1 - prob2 - prob3;
- if(prob > max_prob) max_prob = prob;
- }
- else
- {
- FCL_REAL d1 = projected_p.dot(edge_n[0]) - edge_t[0];
- FCL_REAL d2 = projected_p.dot(edge_n[1]) - edge_t[1];
- FCL_REAL d3 = projected_p.dot(edge_n[2]) - edge_t[2];
-
- std::vector<int> pos_plane;
- std::vector<int> neg_plane;
- if(d1 > 0) pos_plane.push_back(0); else neg_plane.push_back(0);
- if(d2 > 0) pos_plane.push_back(1); else neg_plane.push_back(1);
- if(d3 > 0) pos_plane.push_back(2); else neg_plane.push_back(2);
-
- if(pos_plane.size() == 1)
- {
- int pos_id = pos_plane[0];
- FCL_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[pos_id]) - edge_t[pos_id]) / sqrt(quadraticForm(newS, edge_n[pos_id])));
-
- int neg_id1 = neg_plane[0];
- int neg_id2 = neg_plane[1];
- FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[neg_id1]) - edge_t[neg_id1]) / sqrt(quadraticForm(newS, edge_n[neg_id2])));
- FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[neg_id2]) - edge_t[neg_id2]) / sqrt(quadraticForm(newS, edge_n[neg_id2])));
-
- FCL_REAL prob = prob1 - prob2 - prob3;
- if(prob > max_prob) max_prob = prob;
-
- }
- else if(pos_plane.size() == 2)
- {
- int neg_id = neg_plane[0];
- FCL_REAL prob1 = gaussianCDF(-(projected_p.dot(edge_n[neg_id]) - edge_t[neg_id]) / sqrt(quadraticForm(newS, edge_n[neg_id])));
-
- int pos_id1 = pos_plane[0];
- int pos_id2 = pos_plane[1];
-
- FCL_REAL prob2 = gaussianCDF((projected_p.dot(edge_n[pos_id1])) / sqrt(quadraticForm(newS, edge_n[pos_id1])));
- FCL_REAL prob3 = gaussianCDF((projected_p.dot(edge_n[pos_id2])) / sqrt(quadraticForm(newS, edge_n[pos_id2])));
-
- FCL_REAL prob = prob1 - prob2 - prob3;
- if(prob > max_prob) max_prob = prob;
- }
- else
- {
- std::cerr << "Ooops, seems something is wrong: " << pos_plane.size() << std::endl;
- }
- }
- }
-
- return max_prob;
-}
-
-
-FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
- const Matrix3f& R, const Vec3f& T)
-{
- Vec3f Q1_ = R * Q1 + T;
- Vec3f Q2_ = R * Q2 + T;
- Vec3f Q3_ = R * Q3 + T;
-
- return intersect_PointCloudsTriangle(cloud1, uc1, size_cloud1, Q1_, Q2_, Q3_);
-}
-
-#endif
-
void TriangleDistance::segPoints(const Vec3f& P, const Vec3f& A, const Vec3f& Q, const Vec3f& B,
Vec3f& VEC, Vec3f& X, Vec3f& Y)
diff --git a/trunk/fcl/src/transform.cpp b/trunk/fcl/src/math/transform.cpp
similarity index 91%
rename from trunk/fcl/src/transform.cpp
rename to trunk/fcl/src/math/transform.cpp
index 4a597ec2..cb2e1562 100644
--- a/trunk/fcl/src/transform.cpp
+++ b/trunk/fcl/src/math/transform.cpp
@@ -35,7 +35,7 @@
/** \author Jia Pan */
-#include "fcl/transform.h"
+#include "fcl/math/transform.h"
namespace fcl
{
@@ -63,11 +63,11 @@ void Quaternion3f::fromRotation(const Matrix3f& R)
int i = 0;
if(R(1, 1) > R(0, 0))
{
- i = 1;
+ i = 1;
}
if(R(2, 2) > R(i, i))
{
- i = 2;
+ i = 2;
}
int j = next[i];
int k = next[j];
@@ -129,11 +129,11 @@ void Quaternion3f::fromAxes(const Vec3f axis[3])
int i = 0;
if(axis[1][1] > axis[0][0])
{
- i = 1;
+ i = 1;
}
if(axis[2][2] > axis[i][i])
{
- i = 2;
+ i = 2;
}
int j = next[i];
int k = next[j];
@@ -207,7 +207,7 @@ FCL_REAL Quaternion3f::dot(const Quaternion3f& other) const
Quaternion3f Quaternion3f::operator + (const Quaternion3f& other) const
{
return Quaternion3f(data[0] + other.data[0], data[1] + other.data[1],
- data[2] + other.data[2], data[3] + other.data[3]);
+ data[2] + other.data[2], data[3] + other.data[3]);
}
const Quaternion3f& Quaternion3f::operator += (const Quaternion3f& other)
@@ -223,7 +223,7 @@ const Quaternion3f& Quaternion3f::operator += (const Quaternion3f& other)
Quaternion3f Quaternion3f::operator - (const Quaternion3f& other) const
{
return Quaternion3f(data[0] - other.data[0], data[1] - other.data[1],
- data[2] - other.data[2], data[3] - other.data[3]);
+ data[2] - other.data[2], data[3] - other.data[3]);
}
const Quaternion3f& Quaternion3f::operator -= (const Quaternion3f& other)
@@ -239,9 +239,9 @@ const Quaternion3f& Quaternion3f::operator -= (const Quaternion3f& other)
Quaternion3f Quaternion3f::operator * (const Quaternion3f& other) const
{
return Quaternion3f(data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3],
- data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2],
- data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1],
- data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]);
+ data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2],
+ data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1],
+ data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]);
}
@@ -327,6 +327,19 @@ Quaternion3f inverse(const Quaternion3f& q)
return res.inverse();
}
+const Matrix3f& Transform3f::getRotationInternal() const
+{
+ boost::mutex::scoped_lock slock(const_cast<boost::mutex&>(lock_));
+ if(!matrix_set)
+ {
+ q.toRotation(R);
+ matrix_set = true;
+ }
+
+ return R;
+}
+
+
Transform3f inverse(const Transform3f& tf)
{
Transform3f res(tf);
diff --git a/trunk/fcl/src/profile.cpp b/trunk/fcl/src/profile.cpp
index d0bf88ec..12a8f274 100644
--- a/trunk/fcl/src/profile.cpp
+++ b/trunk/fcl/src/profile.cpp
@@ -35,9 +35,10 @@
/** \author Ioan Sucan */
-#include "ompl/tools/debug/Profiler.h"
+#include "fcl/profile.h"
-ompl::tools::Profiler& ompl::tools::Profiler::Instance(void)
+
+fcl::tools::Profiler& fcl::tools::Profiler::Instance(void)
{
static Profiler p(true, false);
return p;
@@ -45,12 +46,11 @@ ompl::tools::Profiler& ompl::tools::Profiler::Instance(void)
#if ENABLE_PROFILING
-#include "ompl/util/Console.h"
#include <vector>
#include <algorithm>
#include <sstream>
-void ompl::tools::Profiler::start(void)
+void fcl::tools::Profiler::start(void)
{
lock_.lock();
if (!running_)
@@ -61,7 +61,7 @@ void ompl::tools::Profiler::start(void)
lock_.unlock();
}
-void ompl::tools::Profiler::stop(void)
+void fcl::tools::Profiler::stop(void)
{
lock_.lock();
if (running_)
@@ -72,7 +72,7 @@ void ompl::tools::Profiler::stop(void)
lock_.unlock();
}
-void ompl::tools::Profiler::clear(void)
+void fcl::tools::Profiler::clear(void)
{
lock_.lock();
data_.clear();
@@ -82,14 +82,14 @@ void ompl::tools::Profiler::clear(void)
lock_.unlock();
}
-void ompl::tools::Profiler::event(const std::string &name, const unsigned int times)
+void fcl::tools::Profiler::event(const std::string &name, const unsigned int times)
{
lock_.lock();
data_[boost::this_thread::get_id()].events[name] += times;
lock_.unlock();
}
-void ompl::tools::Profiler::average(const std::string &name, const double value)
+void fcl::tools::Profiler::average(const std::string &name, const double value)
{
lock_.lock();
AvgInfo &a = data_[boost::this_thread::get_id()].avg[name];
@@ -99,21 +99,21 @@ void ompl::tools::Profiler::average(const std::string &name, const double value)
lock_.unlock();
}
-void ompl::tools::Profiler::begin(const std::string &name)
+void fcl::tools::Profiler::begin(const std::string &name)
{
lock_.lock();
data_[boost::this_thread::get_id()].time[name].set();
lock_.unlock();
}
-void ompl::tools::Profiler::end(const std::string &name)
+void fcl::tools::Profiler::end(const std::string &name)
{
lock_.lock();
data_[boost::this_thread::get_id()].time[name].update();
lock_.unlock();
}
-void ompl::tools::Profiler::status(std::ostream &out, bool merge)
+void fcl::tools::Profiler::status(std::ostream &out, bool merge)
{
stop();
lock_.lock();
@@ -157,16 +157,9 @@ void ompl::tools::Profiler::status(std::ostream &out, bool merge)
lock_.unlock();
}
-void ompl::tools::Profiler::console(void)
-{
- std::stringstream ss;
- ss << std::endl;
- status(ss, true);
- // logInform(ss.str().c_str());
-}
/// @cond IGNORE
-namespace ompl
+namespace fcl
{
struct dataIntVal
@@ -199,7 +192,7 @@ struct SortDoubleByValue
}
/// @endcond
-void ompl::tools::Profiler::printThreadInfo(std::ostream &out, const PerThread &data)
+void fcl::tools::Profiler::printThreadInfo(std::ostream &out, const PerThread &data)
{
double total = time::seconds(tinfo_.total);
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 7b136fbf..637d5f14 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -116,7 +116,7 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
{
AABB overlap_part;
AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), overlap_part);
- result.addCostSource(CostSource(overlap_part, cost_density));
+ result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
else if((!model1->isFree() && !model2->isFree()) && request.enable_cost)
@@ -125,7 +125,7 @@ static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
{
AABB overlap_part;
AABB(tf1.transform(p1), tf1.transform(p2), tf1.transform(p3)).overlap(AABB(tf2.transform(q1), tf2.transform(q2), tf2.transform(q3)), overlap_part);
- result.addCostSource(CostSource(overlap_part, cost_density));
+ result.addCostSource(CostSource(overlap_part, cost_density), request.num_max_cost_sources);
}
}
}
@@ -292,188 +292,6 @@ void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
}
-#if USE_SVMLIGHT
-
-
-namespace details
-{
-template<typename BV>
-static inline void pointCloudCollisionOrientedNodeLeafTesting(int b1, int b2,
- const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- Vec3f* vertices1, Vec3f* vertices2,
- const Matrix3f& R, const Vec3f& T,
- bool enable_statistics,
- FCL_REAL collision_prob_threshold,
- const boost::shared_arry<Variance3f>& uc1, const boost::shared_array<Variance3f>& uc2,
- const CloudClassifierParam classifier_param,
- int& num_leaf_tests,
- FCL_REAL& max_collision_prob,
- std::vector<BVHPointCollisionPair>& pairs)
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<BV>& node1 = model1->getBV(b1);
- const BVNode<BV>& node2 = model2->getBV(b2);
-
- FCL_REAL collision_prob = Intersect::intersect_PointClouds(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- vertices2 + node2.first_primitive, uc2.get() + node2.first_primitive,
- node2.num_primitives,
- R, T,
- classifier_param);
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
-}
-
-}
-
-PointCloudCollisionTraversalNodeOBB::PointCloudCollisionTraversalNodeOBB() : PointCloudCollisionTraversalNode<OBB>()
-{
- R.setIdentity();
- // default T is 0
-}
-
-bool PointCloudCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void PointCloudCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
-{
- details::pointCloudCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- R, T,
- enable_statistics,
- collision_prob_threshold,
- uc1, uc2,
- classifier_param,
- num_leaf_tests,
- max_collision_prob,
- pairs);
-}
-
-PointCloudCollisionTraversalNodeRSS::PointCloudCollisionTraversalNodeRSS() : PointCloudCollisionTraversalNode<RSS>()
-{
- R.setIdentity();
- // default T is 0
-}
-
-bool PointCloudCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void PointCloudCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
-{
- details::pointCloudCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- R, T,
- enable_statistics,
- collision_prob_threshold,
- uc1, uc2,
- classifier_param,
- num_leaf_tests,
- max_collision_prob,
- pairs);
-}
-
-
-namespace details
-{
-
-template<typename BV>
-static inline void pointCloudMeshCollisionOrientedNodeLeafTesting(int b1, int b2,
- const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- Vec3f* vertices1, Vec3f* vertices2,
- Triangle* tri_indices2,
- const Matrix3f& R, const Vec3f& T,
- bool enable_statistics,
- FCL_REAL collision_prob_threshold,
- const boost::shared_array<Variance3f>& uc1,
- int& num_leaf_tests,
- FCL_REAL& max_collision_prob,
- std::vector<BVHPointCollisionPair>& pairs)
-{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<OBB>& node1 = model1->getBV(b1);
- const BVNode<OBB>& node2 = model2->getBV(b2);
-
-
- const Triangle& tri_id2 = tri_indices2[node2.primitiveId()];
-
- const Vec3f& q1 = vertices2[tri_id2[0]];
- const Vec3f& q2 = vertices2[tri_id2[1]];
- const Vec3f& q3 = vertices2[tri_id2[2]];
-
- FCL_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- q1, q2, q3,
- R, T);
-
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
-
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
-}
-
-}
-
-
-PointCloudMeshCollisionTraversalNodeOBB::PointCloudMeshCollisionTraversalNodeOBB() : PointCloudMeshCollisionTraversalNode<OBB>()
-{
- R.setIdentity();
- // default T is 0
-}
-
-bool PointCloudMeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void PointCloudMeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
-{
- details::pointCloudMeshCollisionOrientedNodeLeafTesting(b1, b2,
- model1, model2,
- vertices1, vertices2,
- tri_indices2,
- R, T,
- enable_statistics, collision_prob_threshold, uc1,
- num_leaf_tests, max_collision_prob, pairs);
-}
-
-PointCloudMeshCollisionTraversalNodeRSS::PointCloudMeshCollisionTraversalNodeRSS() : PointCloudMeshCollisionTraversalNode<RSS>()
-{
- R.setIdentity();
- // default T is 0
-}
-
-bool PointCloudMeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- return !overlap(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
-}
-
-void PointCloudMeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
-{
- details::pointCloudMeshCollisionOrientedNodeLeafTesting(b1, b2,
- model1, model2,
- vertices1, vertices2,
- tri_indices2,
- R, T,
- enable_statistics, collision_prob_threshold, uc1,
- num_leaf_tests, max_collision_prob, pairs);
-}
-
-#endif
-
namespace details
{
@@ -610,7 +428,7 @@ void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
}
-/** for OBB and RSS, there is local coordinate of BV, so normal need to be transformed */
+/// for OBB and RSS, there is local coordinate of BV, so normal need to be transformed
template<>
bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
{
@@ -790,9 +608,9 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
}
- /** n is the local frame of object 1, pointing from object 1 to object2 */
+ /// n is the local frame of object 1, pointing from object 1 to object2
Vec3f n = P2 - P1;
- /** turn n into the global frame, pointing from object 1 to object 2 */
+ /// turn n into the global frame, pointing from object 1 to object 2
Matrix3f R0;
motion1->getCurrentRotation(R0);
Vec3f n_transformed = R0 * n;
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/traversal_node_setup.cpp
similarity index 54%
rename from trunk/fcl/src/simple_setup.cpp
rename to trunk/fcl/src/traversal_node_setup.cpp
index 2f3ac7fb..46a25c22 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/traversal_node_setup.cpp
@@ -35,7 +35,7 @@
/** \author Jia Pan */
-#include "fcl/simple_setup.h"
+#include "fcl/traversal_node_setup.h"
namespace fcl
{
@@ -66,7 +66,6 @@ static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
node.request = request;
node.result = &result;
- result.setRequest(request);
node.cost_density = model1.cost_density * model2.cost_density;
@@ -117,141 +116,6 @@ bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
}
-#if USE_SVMLIGHT
-
-namespace details
-{
-template<typename BV, typename OrientedNode>
-static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
- BVHModel<BV>& model1, const Transform3f& tf1,
- BVHModel<BV>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold,
- int leaf_size_threshold,
- FCL_REAL expand_r)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
- || !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.uc1.reset(new Variance3f[model1.num_vertices]);
- node.uc2.reset(new Variance3f[model2.num_vertices]);
-
- estimateSamplingVariance(model1.vertices, model1.num_vertices, node.uc1.get());
- estimateSamplingVariance(model2.vertices, model2.num_vertices, node.uc2.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
- BVHExpand(model2, node.uc2.get(), expand_r);
-
- node.request = request;
-
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
-
- return true;
-}
-
-}
-
-bool initialize(PointCloudCollisionTraversalNodeOBB& node,
- BVHModel<OBB>& model1, const Transform3f& tf1,
- BVHModel<OBB>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold,
- int leaf_size_threshold,
- FCL_REAL expand_r)
-{
- return details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, request, collision_prob_threshold, leaf_size_threshold, expand_r);
-}
-
-
-bool initialize(PointCloudCollisionTraversalNodeRSS& node,
- BVHModel<RSS>& model1, const Transform3f& tf1,
- BVHModel<RSS>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold,
- int leaf_size_threshold,
- FCL_REAL expand_r)
-{
- return details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, request, collision_prob_threshold, leaf_size_threshold, expand_r);
-}
-
-namespace details
-{
-
-template<typename BV, typename OrientedNode>
-static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
- BVHModel<BV>& model1, const Transform3f& tf1,
- const BVHModel<BV>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold,
- int leaf_size_threshold,
- FCL_REAL expand_r)
-{
- if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices2 = model2.tri_indices;
- node.uc1.reset(new Variance3f[model1.num_vertices]);
-
- estimateSamplingVariance(model1.vertices, model1.num_vertices, node.uc1.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
-
- node.request = request;
- node.collision_prob_threshold = collision_prob_threshold;
- node.leaf_size_threshold = leaf_size_threshold;
-
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
-
- return true;
-}
-}
-
-bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
- BVHModel<OBB>& model1, const Transform3f& tf1,
- const BVHModel<OBB>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold,
- int leaf_size_threshold,
- FCL_REAL expand_r)
-{
- return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, collision_prob_threshold, leaf_size_threshold, expand_r);
-}
-
-
-bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
- BVHModel<RSS>& model1, const Transform3f& tf1,
- const BVHModel<RSS>& model2, const Transform3f& tf2,
- const CollisionRequest& request,
- FCL_REAL collision_prob_threshold,
- int leaf_size_threshold,
- FCL_REAL expand_r)
-{
- return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, request, collision_prob_threshold, leaf_size_threshold, expand_r);
-}
-
-#endif
-
-
namespace details
{
template<typename BV, typename OrientedNode>
@@ -266,7 +130,6 @@ static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
node.request = request;
node.result = &result;
- result.setRequest(request);
node.model1 = &model1;
node.tf1 = tf1;
--
GitLab