diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 780aca93c7fd1e416bacd1e77e40a83897b737cd..bd9bd47241c63e4b8a35463ea6da49b4fea106a8 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -37,7 +37,7 @@ link_directories(${CCD_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/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/gjk_libccd.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.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/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/gjk_libccd.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/BV/OBBRSS.h b/trunk/fcl/include/fcl/BV/OBBRSS.h
index 53f7689d70d45c8dac7b319e2adb5ab32d0f776a..d8d9050b0050eee5622b57433e887c729cff268d 100644
--- a/trunk/fcl/include/fcl/BV/OBBRSS.h
+++ b/trunk/fcl/include/fcl/BV/OBBRSS.h
@@ -40,7 +40,8 @@
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
#include "fcl/matrix_3f.h"
-
+#include "fcl/BV/OBB.h"
+#include "fcl/BV/RSS.h"
namespace fcl
{
@@ -127,15 +128,9 @@ public:
};
-static bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
-{
- return overlap(R0, T0, b1.obb, b2.obb);
-}
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
-static BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL)
-{
- return distance(R0, T0, b1.rss, b2.rss, P, Q);
-}
+BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
}
diff --git a/trunk/fcl/include/fcl/intersect.h b/trunk/fcl/include/fcl/intersect.h
index 80ec05efe47fe8d2198dc2c6374cf24da9660273..87f3044216f7fb19eff452f33258a1627cfaf765 100644
--- a/trunk/fcl/include/fcl/intersect.h
+++ b/trunk/fcl/include/fcl/intersect.h
@@ -44,8 +44,8 @@
#if USE_SVMLIGHT
extern "C"
{
-# include <svm_light/svm_common.h>
-# include <svm_light/svm_learn.h>
+#include <svm_light/svm_common.h>
+#include <svm_light/svm_learn.h>
}
#endif
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index ee9647b35a0817c50c3be2c16a82819ae7992dcd..59bc0d3e813a48b589b3e3ef1aba809e90d360b0 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -158,14 +158,15 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
}
+namespace details
+{
-/** \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 SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver,
- int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+template<typename BV, typename S, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
+static inline bool setupMeshShapeCollisionOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
+ const BVHModel<BV>& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
+ int num_max_contacts, bool exhaustive, bool enable_contact)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -190,39 +191,21 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
return true;
}
+}
+
+
/** \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 SimpleTransform& tf1,
- const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
+ const BVHModel<OBB>& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
- if(model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model1, tf1, node.model1_bv);
-
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- node.R = tf2.getRotation();
- node.T = tf2.getTranslation();
-
- return true;
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
}
-
/** \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,
@@ -231,37 +214,41 @@ bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model2, tf2, node.model2_bv);
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- node.R = tf1.getRotation();
- node.T = tf1.getTranslation();
-
- return true;
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
}
+/** \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 SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
+ int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+{
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+}
-/** \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 OBBRSS type */
template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
+ const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
+{
+ return details::setupMeshShapeCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+}
+
+
+
+namespace details
+{
+template<typename S, typename BV, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
+static inline bool setupShapeMeshCollisionOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
+ int num_max_contacts, bool exhaustive, bool enable_contact)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -285,104 +272,42 @@ bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
return true;
}
+}
-/** \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 OBB type */
template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
- const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const BVHModel<OBB>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model2, tf2, node.model2_bv);
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- node.R = tf1.getRotation();
- node.T = tf1.getTranslation();
-
- return true;
+ return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
}
-
-/** \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 RSS type */
template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
+bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const BVHModel<RSS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
- if(model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model1, tf1, node.model1_bv);
-
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- node.R = tf2.getRotation();
- node.T = tf2.getTranslation();
-
- return true;
+ return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
}
-
-/** \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 kIOS type */
template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
- const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model2, tf2, node.model2_bv);
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- node.R = tf1.getRotation();
- node.T = tf1.getTranslation();
-
- return true;
+ return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
}
-
/** \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,
@@ -391,27 +316,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& nod
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
- if(model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model1, tf1, node.model1_bv);
-
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- node.R = tf2.getRotation();
- node.T = tf2.getTranslation();
-
- return true;
+ return setupShapeMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
}
@@ -875,12 +780,14 @@ bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
}
+namespace details
+{
-template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver)
+template<typename BV, typename S, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
+static inline bool setupMeshShapeDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
+ const BVHModel<BV>& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -900,65 +807,44 @@ bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
return true;
}
+}
template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
+ const BVHModel<RSS>& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver)
{
- if(model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model1, tf1, node.model1_bv);
-
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
- node.R = tf2.getRotation();
- node.T = tf2.getTranslation();
-
- return true;
+ return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
}
-
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model2, tf2, node.model2_bv);
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
- node.R = tf1.getRotation();
- node.T = tf1.getTranslation();
-
- return true;
+ return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
}
-
template<typename S, typename NarrowPhaseSolver>
-bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
- const S& model1, const SimpleTransform& tf1,
- const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
+ const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver)
+{
+ return details::setupMeshShapeDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
+}
+
+
+namespace details
+{
+template<typename S, typename BV, typename NarrowPhaseSolver, template<typename, typename> class OrientedNode>
+static inline bool setupShapeMeshDistanceOrientedNode(OrientedNode<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -976,35 +862,29 @@ bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
node.R = tf2.getRotation();
node.T = tf2.getTranslation();
- return true;
+ return true;
+}
}
+
template<typename S, typename NarrowPhaseSolver>
-bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
- const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
- const S& model2, const SimpleTransform& tf2,
+bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const BVHModel<RSS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model2, tf2, node.model2_bv);
-
- node.vertices = model1.vertices;
- node.tri_indices = model1.tri_indices;
- node.R = tf1.getRotation();
- node.T = tf1.getTranslation();
-
- return true;
+ return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
}
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
+{
+ return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
+}
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
@@ -1012,23 +892,7 @@ bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver)
{
- if(model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
- node.nsolver = nsolver;
-
- computeBV(model1, tf1, node.model1_bv);
-
- node.vertices = model2.vertices;
- node.tri_indices = model2.tri_indices;
- node.R = tf2.getRotation();
- node.T = tf2.getTranslation();
-
- return true;
+ return details::setupShapeMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, nsolver);
}
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index ccbeae927ecc1ea67acc9f03a64b32ba645be510..72782ba7d6a93e59f7394781c1aeb8cdbc6bf347 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -241,19 +241,21 @@ public:
};
+namespace details
+{
template<typename BV, typename S, typename NarrowPhaseSolver>
-static inline void meshShapeCollisionLeafTesting(int b1, int b2,
- const BVHModel<BV>* model1, const S& model2,
- Vec3f* vertices, Triangle* tri_indices,
- const Matrix3f& R, const Vec3f& T,
- const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver,
- bool enable_statistics,
- bool enable_contact,
- bool exhaustive,
- int num_max_contacts,
- int& num_leaf_tests,
- std::vector<BVHShapeCollisionPair>& pairs)
+static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
+ const BVHModel<BV>* model1, const S& model2,
+ Vec3f* vertices, Triangle* tri_indices,
+ const Matrix3f& R, const Vec3f& T,
+ const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
+ bool enable_statistics,
+ bool enable_contact,
+ bool exhaustive,
+ int num_max_contacts,
+ int& num_leaf_tests,
+ std::vector<BVHShapeCollisionPair>& pairs)
{
if(enable_statistics) num_leaf_tests++;
@@ -287,6 +289,8 @@ static inline void meshShapeCollisionLeafTesting(int b1, int b2,
}
}
+}
+
template<typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
{
@@ -304,9 +308,9 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeCollisionLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
@@ -331,9 +335,9 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeCollisionLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
@@ -358,9 +362,9 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeCollisionLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
@@ -385,9 +389,9 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeCollisionLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
}
// R, T is the transform of bvh model
@@ -478,9 +482,9 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeCollisionLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
@@ -508,9 +512,9 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeCollisionLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
@@ -538,9 +542,9 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeCollisionLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
@@ -568,9 +572,9 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeCollisionLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
- this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
// may need to change the order in pairs
}
@@ -732,18 +736,20 @@ public:
const NarrowPhaseSolver* nsolver;
};
+namespace details
+{
template<typename BV, typename S, typename NarrowPhaseSolver>
-void meshShapeDistanceLeafTesting(int b1, int b2,
- const BVHModel<BV>* model1, const S& model2,
- Vec3f* vertices, Triangle* tri_indices,
- const Matrix3f&R, const Vec3f& T,
- const SimpleTransform& tf2,
- const NarrowPhaseSolver* nsolver,
- bool enable_statistics,
- int & num_leaf_tests,
- int& last_tri_id,
- BVH_REAL& min_distance)
+void meshShapeDistanceOrientedNodeLeafTesting(int b1, int b2,
+ const BVHModel<BV>* model1, const S& model2,
+ Vec3f* vertices, Triangle* tri_indices,
+ const Matrix3f&R, const Vec3f& T,
+ const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
+ bool enable_statistics,
+ int & num_leaf_tests,
+ int& last_tri_id,
+ BVH_REAL& min_distance)
{
if(enable_statistics) num_leaf_tests++;
@@ -767,6 +773,8 @@ void meshShapeDistanceLeafTesting(int b1, int b2,
}
}
+}
+
template<typename S, typename NarrowPhaseSolver>
static inline void distance_preprocess(Vec3f* vertices, Triangle* tri_indices, int last_tri_id,
@@ -824,8 +832,8 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeDistanceLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
Matrix3f R;
@@ -860,8 +868,8 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeDistanceLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
Matrix3f R;
@@ -895,8 +903,8 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeDistanceLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ details::meshShapeDistanceOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
Matrix3f R;
@@ -994,8 +1002,8 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeDistanceLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
Matrix3f R;
@@ -1029,8 +1037,8 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeDistanceLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
Matrix3f R;
@@ -1064,8 +1072,8 @@ public:
void leafTesting(int b1, int b2) const
{
- meshShapeDistanceLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
- R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ details::meshShapeDistanceOrientedNodeLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
}
Matrix3f R;
diff --git a/trunk/fcl/manifest.xml b/trunk/fcl/manifest.xml
index c9b5c18758c9e354eef3eb7791498073a8c26a50..d7c37367f0bb50092b5a51c0f1f503fdedc24ab0 100644
--- a/trunk/fcl/manifest.xml
+++ b/trunk/fcl/manifest.xml
@@ -9,11 +9,11 @@
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/fcl</url>
- <!-- <depend package="ann"/> -->
<depend package="common_rosdeps"/>
<rosdep name="flann" />
<rosdep name="libccd" />
+
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lfcl"/>
</export>
diff --git a/trunk/fcl/src/BV/OBBRSS.cpp b/trunk/fcl/src/BV/OBBRSS.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c821df0523324d2cfadcd8da72f4a21bf256e2b0
--- /dev/null
+++ b/trunk/fcl/src/BV/OBBRSS.cpp
@@ -0,0 +1,53 @@
+/*
+ * 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/BV/OBBRSS.h"
+
+namespace fcl
+{
+
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
+{
+ return overlap(R0, T0, b1.obb, b2.obb);
+}
+
+BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P, Vec3f* Q)
+{
+ return distance(R0, T0, b1.rss, b2.rss, P, Q);
+}
+
+
+}
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index 5bdc6ac494fbbfee4b13d1aeaf6601980ca37d15..504f3ab3a84df2d63320818d83c0651112f5b533 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -254,6 +254,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane, Convex, NarrowPhaseSolver>;
distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane, NarrowPhaseSolver>;
+ /* AABB distance not implemented */
/*
distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB, Sphere, NarrowPhaseSolver>::distance;
@@ -280,6 +281,7 @@ DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS, Convex, NarrowPhaseSolver>::distance;
distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS, Plane, NarrowPhaseSolver>::distance;
+ /* KDOP distance not implemented */
/*
distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<16>, Box, NarrowPhaseSolver>::distance;
distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<16>, Sphere, NarrowPhaseSolver>::distance;
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index 1549e1779ba60e28fbb38f9b4a39717c48e4bc2d..6aa46a9dfe9ac066c9b9c6ef1324a107732eb372 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -39,10 +39,15 @@
namespace fcl
{
-bool initialize(MeshCollisionTraversalNodeOBB& node,
- const BVHModel<OBB>& model1, const SimpleTransform& tf1,
- const BVHModel<OBB>& model2, const SimpleTransform& tf2,
- int num_max_contacts, bool exhaustive, bool enable_contact)
+
+
+namespace details
+{
+template<typename BV, typename OrientedNode>
+static inline bool setupMeshCollisionOrientedNode(OrientedNode& node,
+ const BVHModel<BV>& model1, const SimpleTransform& tf1,
+ const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ int num_max_contacts, bool exhaustive, bool enable_contact)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -67,34 +72,24 @@ bool initialize(MeshCollisionTraversalNodeOBB& node,
return true;
}
+}
+
+
+bool initialize(MeshCollisionTraversalNodeOBB& node,
+ const BVHModel<OBB>& model1, const SimpleTransform& tf1,
+ const BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ int num_max_contacts, bool exhaustive, bool enable_contact)
+{
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, num_max_contacts, exhaustive, enable_contact);
+}
+
bool initialize(MeshCollisionTraversalNodeRSS& node,
const BVHModel<RSS>& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
int num_max_contacts, bool exhaustive, bool enable_contact)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
-
- return true;
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, num_max_contacts, exhaustive, enable_contact);
}
@@ -103,27 +98,7 @@ bool initialize(MeshCollisionTraversalNodekIOS& node,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
int num_max_contacts, bool exhaustive, bool enable_contact)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
-
- return true;
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, num_max_contacts, exhaustive, enable_contact);
}
bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
@@ -131,41 +106,24 @@ bool initialize(MeshCollisionTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
int num_max_contacts, bool exhaustive, bool enable_contact)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.model1 = &model1;
- node.tf1 = tf1;
- node.model2 = &model2;
- node.tf2 = tf2;
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
-
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
-
- return true;
+ return details::setupMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, num_max_contacts, exhaustive, enable_contact);
}
#if USE_SVMLIGHT
-bool initialize(PointCloudCollisionTraversalNodeOBB& node,
- BVHModel<OBB>& model1, const SimpleTransform& tf1,
- BVHModel<OBB>& model2, const SimpleTransform& tf2,
- BVH_REAL collision_prob_threshold,
- int leaf_size_threshold,
- int num_max_contacts,
- bool exhaustive,
- bool enable_contact,
- BVH_REAL expand_r)
+namespace details
+{
+template<typename BV, typename OrientedNode>
+static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
+ BVHModel<BV>& model1, const SimpleTransform& tf1,
+ BVHModel<BV>& model2, const SimpleTransform& tf2,
+ BVH_REAL collision_prob_threshold,
+ int leaf_size_threshold,
+ int num_max_contacts,
+ bool exhaustive,
+ bool enable_contact,
+ BVH_REAL expand_r)
{
if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD)
|| !(model2.getModelType() == BVH_MODEL_TRIANGLES || model2.getModelType() == BVH_MODEL_POINTCLOUD))
@@ -196,7 +154,22 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node,
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
- return true;
+ return true;
+}
+
+}
+
+bool initialize(PointCloudCollisionTraversalNodeOBB& node,
+ BVHModel<OBB>& model1, const SimpleTransform& tf1,
+ BVHModel<OBB>& model2, const SimpleTransform& tf2,
+ BVH_REAL collision_prob_threshold,
+ int leaf_size_threshold,
+ int num_max_contacts,
+ bool exhaustive,
+ bool enable_contact,
+ BVH_REAL expand_r)
+{
+ return details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
}
@@ -210,8 +183,24 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node,
bool enable_contact,
BVH_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 details::setupPointCloudCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
+}
+
+namespace details
+{
+
+template<typename BV, typename OrientedNode>
+static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
+ BVHModel<BV>& model1, const SimpleTransform& tf1,
+ const BVHModel<BV>& model2, const SimpleTransform& tf2,
+ BVH_REAL collision_prob_threshold,
+ int leaf_size_threshold,
+ int num_max_contacts,
+ bool exhaustive,
+ bool enable_contact,
+ BVH_REAL expand_r)
+{
+ if(!(model1.getModelType() == BVH_MODEL_TRIANGLES || model1.getModelType() == BVH_MODEL_POINTCLOUD) || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
@@ -222,14 +211,12 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node,
node.vertices1 = model1.vertices;
node.vertices2 = model2.vertices;
+ node.tri_indices2 = model2.tri_indices;
node.uc1.reset(new Uncertainty[model1.num_vertices]);
- node.uc2.reset(new Uncertainty[model2.num_vertices]);
estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
- estimateSamplingUncertainty(model2.vertices, model2.num_vertices, node.uc2.get());
BVHExpand(model1, node.uc1.get(), expand_r);
- BVHExpand(model2, node.uc2.get(), expand_r);
node.num_max_contacts = num_max_contacts;
node.exhaustive = exhaustive;
@@ -240,6 +227,7 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node,
relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
return true;
+}
}
bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
@@ -252,33 +240,7 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node,
bool enable_contact,
BVH_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 Uncertainty[model1.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- 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;
+ return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
}
@@ -292,40 +254,18 @@ bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
bool enable_contact,
BVH_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 Uncertainty[model1.num_vertices]);
-
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
-
- BVHExpand(model1, node.uc1.get(), expand_r);
-
- node.num_max_contacts = num_max_contacts;
- node.exhaustive = exhaustive;
- node.enable_contact = enable_contact;
- 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;
+ return details::setupPointCloudMeshCollisionOrientedNode(node, model1, tf1, model2, tf2, collision_prob_threshold, leaf_size_threshold, num_max_contacts, exhaustive, enable_contact, expand_r);
}
#endif
-bool initialize(MeshDistanceTraversalNodeRSS& node,
- const BVHModel<RSS>& model1, const SimpleTransform& tf1,
- const BVHModel<RSS>& model2, const SimpleTransform& tf2)
+
+namespace details
+{
+template<typename BV, typename OrientedNode>
+static inline bool setupMeshDistanceOrientedNode(OrientedNode& node,
+ const BVHModel<BV>& model1, const SimpleTransform& tf1,
+ const BVHModel<BV>& model2, const SimpleTransform& tf2)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
@@ -347,50 +287,28 @@ bool initialize(MeshDistanceTraversalNodeRSS& node,
}
+}
+
+bool initialize(MeshDistanceTraversalNodeRSS& node,
+ const BVHModel<RSS>& model1, const SimpleTransform& tf1,
+ const BVHModel<RSS>& model2, const SimpleTransform& tf2)
+{
+ return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2);
+}
+
+
bool initialize(MeshDistanceTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || 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_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
-
- return true;
+ return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2);
}
bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2)
{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || 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_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
-
- return true;
+ return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2);
}
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 86cec884fd9e4939bf350d72eb72353da701b182..ca74672a23b8e84afa0dc4fd86e0d94993df530d 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -40,18 +40,20 @@
namespace fcl
{
+namespace details
+{
template<typename BV>
-static inline void meshCollisionLeafTesting(int b1, int b2,
- const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- Vec3f* vertices1, Vec3f* vertices2,
- Triangle* tri_indices1, Triangle* tri_indices2,
- const Matrix3f& R, const Vec3f& T,
- bool enable_statistics,
- bool enable_contact,
- bool exhaustive,
- int num_max_contacts,
- int& num_leaf_tests,
- std::vector<BVHCollisionPair>& pairs)
+static inline void meshCollisionOrientedNodeLeafTesting(int b1, int b2,
+ const BVHModel<BV>* model1, const BVHModel<BV>* model2,
+ Vec3f* vertices1, Vec3f* vertices2,
+ Triangle* tri_indices1, Triangle* tri_indices2,
+ const Matrix3f& R, const Vec3f& T,
+ bool enable_statistics,
+ bool enable_contact,
+ bool exhaustive,
+ int num_max_contacts,
+ int& num_leaf_tests,
+ std::vector<BVHCollisionPair>& pairs)
{
if(enable_statistics) num_leaf_tests++;
@@ -101,19 +103,21 @@ static inline void meshCollisionLeafTesting(int b1, int b2,
}
+
+
template<typename BV>
-static inline void meshDistanceLeafTesting(int b1, int b2,
- const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- Vec3f* vertices1, Vec3f* vertices2,
- Triangle* tri_indices1, Triangle* tri_indices2,
- const Matrix3f& R, const Vec3f& T,
- bool enable_statistics,
- int& num_leaf_tests,
- Vec3f& p1,
- Vec3f& p2,
- int& last_tri_id1,
- int& last_tri_id2,
- BVH_REAL& min_distance)
+static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
+ const BVHModel<BV>* model1, const BVHModel<BV>* model2,
+ Vec3f* vertices1, Vec3f* vertices2,
+ Triangle* tri_indices1, Triangle* tri_indices2,
+ const Matrix3f& R, const Vec3f& T,
+ bool enable_statistics,
+ int& num_leaf_tests,
+ Vec3f& p1,
+ Vec3f& p2,
+ int& last_tri_id1,
+ int& last_tri_id2,
+ BVH_REAL& min_distance)
{
if(enable_statistics) num_leaf_tests++;
@@ -153,7 +157,7 @@ static inline void meshDistanceLeafTesting(int b1, int b2,
}
}
-
+}
MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB() : MeshCollisionTraversalNode<OBB>()
{
@@ -169,13 +173,13 @@ bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
{
- fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
@@ -187,13 +191,13 @@ bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2, const Matrix3f& Rc
void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const
{
- fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
@@ -212,13 +216,13 @@ bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
{
- fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
@@ -238,13 +242,13 @@ bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const
void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
{
- fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
@@ -263,37 +267,39 @@ bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
{
- fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ details::meshCollisionOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
#if USE_SVMLIGHT
-PointCloudCollisionTraversalNodeOBB::PointCloudCollisionTraversalNodeOBB() : PointCloudCollisionTraversalNode<OBB>()
-{
- R.setIdentity();
- // default T is 0
-}
-bool PointCloudCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
+namespace details
{
- 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
+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,
+ BVH_REAL collision_prob_threshold,
+ const boost::shared_arry<Uncertainty>& uc1, const boost::shared_array<Uncertainty>& uc2,
+ const CloudClassifierParam classifier_param,
+ int& num_leaf_tests,
+ BVH_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 BVNode<BV>& node1 = model1->getBV(b1);
+ const BVNode<BV>& node2 = model2->getBV(b2);
+
BVH_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,
@@ -301,7 +307,6 @@ void PointCloudCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
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));
@@ -310,54 +315,74 @@ void PointCloudCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
max_collision_prob = collision_prob;
}
-PointCloudCollisionTraversalNodeRSS::PointCloudCollisionTraversalNodeRSS() : PointCloudCollisionTraversalNode<RSS>()
+}
+
+PointCloudCollisionTraversalNodeOBB::PointCloudCollisionTraversalNodeOBB() : PointCloudCollisionTraversalNode<OBB>()
{
R.setIdentity();
// default T is 0
}
-bool PointCloudCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
+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 PointCloudCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
+void PointCloudCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<RSS>& node1 = model1->getBV(b1);
- const BVNode<RSS>& node2 = model2->getBV(b2);
-
- BVH_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;
+ 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);
}
-
-PointCloudMeshCollisionTraversalNodeOBB::PointCloudMeshCollisionTraversalNodeOBB() : PointCloudMeshCollisionTraversalNode<OBB>()
+PointCloudCollisionTraversalNodeRSS::PointCloudCollisionTraversalNodeRSS() : PointCloudCollisionTraversalNode<RSS>()
{
R.setIdentity();
// default T is 0
}
-bool PointCloudMeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
+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 PointCloudMeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
+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,
+ BVH_REAL collision_prob_threshold,
+ const boost::shared_array<Uncertainty>& uc1,
+ int& num_leaf_tests,
+ BVH_REAL& max_collision_prob,
+ std::vector<BVHPointCollisionPair>& pairs)
{
if(enable_statistics) num_leaf_tests++;
@@ -383,41 +408,53 @@ void PointCloudMeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
max_collision_prob = collision_prob;
}
-PointCloudMeshCollisionTraversalNodeRSS::PointCloudMeshCollisionTraversalNodeRSS() : PointCloudMeshCollisionTraversalNode<RSS>()
+}
+
+
+PointCloudMeshCollisionTraversalNodeOBB::PointCloudMeshCollisionTraversalNodeOBB() : PointCloudMeshCollisionTraversalNode<OBB>()
{
R.setIdentity();
// default T is 0
}
-bool PointCloudMeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
+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 PointCloudMeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
+void PointCloudMeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
{
- if(enable_statistics) num_leaf_tests++;
-
- const BVNode<RSS>& node1 = model1->getBV(b1);
- const BVNode<RSS>& 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]];
+ 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);
+}
- BVH_REAL collision_prob = Intersect::intersect_PointCloudsTriangle(vertices1 + node1.first_primitive, uc1.get() + node1.first_primitive,
- node1.num_primitives,
- q1, q2, q3,
- R, T);
+PointCloudMeshCollisionTraversalNodeRSS::PointCloudMeshCollisionTraversalNodeRSS() : PointCloudMeshCollisionTraversalNode<RSS>()
+{
+ R.setIdentity();
+ // default T is 0
+}
- if(collision_prob > collision_prob_threshold)
- pairs.push_back(BVHPointCollisionPair(node1.first_primitive, node1.num_primitives, node2.first_primitive, node2.num_primitives, collision_prob));
+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);
+}
- if(collision_prob > max_collision_prob)
- max_collision_prob = collision_prob;
+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
@@ -487,9 +524,9 @@ BVH_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const
void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const
{
- fcl::meshDistanceLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, enable_statistics, num_leaf_tests,
- p1, p2, last_tri_id1, last_tri_id2, min_distance);
+ details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
+ R, T, enable_statistics, num_leaf_tests,
+ p1, p2, last_tri_id1, last_tri_id2, min_distance);
}
MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() : MeshDistanceTraversalNode<kIOS>()
@@ -516,9 +553,9 @@ BVH_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const
void MeshDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const
{
- fcl::meshDistanceLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, enable_statistics, num_leaf_tests,
- p1, p2, last_tri_id1, last_tri_id2, min_distance);
+ details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
+ R, T, enable_statistics, num_leaf_tests,
+ p1, p2, last_tri_id1, last_tri_id2, min_distance);
}
MeshDistanceTraversalNodeOBBRSS::MeshDistanceTraversalNodeOBBRSS() : MeshDistanceTraversalNode<OBBRSS>()
@@ -545,9 +582,9 @@ BVH_REAL MeshDistanceTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
{
- fcl::meshDistanceLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, enable_statistics, num_leaf_tests,
- p1, p2, last_tri_id1, last_tri_id2, min_distance);
+ details::meshDistanceOrientedNodeLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
+ R, T, enable_statistics, num_leaf_tests,
+ p1, p2, last_tri_id1, last_tri_id2, min_distance);
}