diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index ba38a833572ffc61f503dfffb546af6e99e14978..c4238bd848969ae2163f49d07a22002b4bf0f865 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/AABB.cpp src/OBB.cpp src/RSS.cpp src/vec_3f.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/geometric_shapes_intersect.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)
+add_library(${PROJECT_NAME} SHARED src/AABB.cpp src/OBB.cpp src/RSS.cpp src/kIOS.cpp src/vec_3f.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/geometric_shapes_intersect.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)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/BVH_model.h b/trunk/fcl/include/fcl/BVH_model.h
index 38aaa40e1e70792ba23f027244a7c7f1762d0adc..9be3c7d1d0763e2c4a9fe8f6dd70a7b7ef48b694 100644
--- a/trunk/fcl/include/fcl/BVH_model.h
+++ b/trunk/fcl/include/fcl/BVH_model.h
@@ -284,6 +284,9 @@ NODE_TYPE BVHModel<OBB>::getNodeType() const;
template<>
NODE_TYPE BVHModel<RSS>::getNodeType() const;
+template<>
+NODE_TYPE BVHModel<kIOS>::getNodeType() const;
+
template<>
NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
diff --git a/trunk/fcl/include/fcl/BVH_utility.h b/trunk/fcl/include/fcl/BVH_utility.h
index 436dcd32cf45229448c6c017daf620f9bda2ae1e..9868b245d72082150ab009a011f0726091948215 100644
--- a/trunk/fcl/include/fcl/BVH_utility.h
+++ b/trunk/fcl/include/fcl/BVH_utility.h
@@ -111,6 +111,15 @@ void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec
*/
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
+/** \brief Compute the center and radius for a triangle's circumcircle */
+void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, BVH_REAL& radius);
+
+/** \brief Compute the maximum distance from a given center point to a point cloud */
+BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query);
+
+BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
+
+
}
#endif
diff --git a/trunk/fcl/include/fcl/BV_fitter.h b/trunk/fcl/include/fcl/BV_fitter.h
index e8238b01ff5d6adfcdc6708993c0fd7d8c2a05aa..9095ec3d39a86c2680d4970039617718001b15b4 100644
--- a/trunk/fcl/include/fcl/BV_fitter.h
+++ b/trunk/fcl/include/fcl/BV_fitter.h
@@ -43,6 +43,7 @@
#include "fcl/vec_3f.h"
#include "fcl/OBB.h"
#include "fcl/RSS.h"
+#include "fcl/kIOS.h"
#include <iostream>
/** \brief Main namespace */
@@ -65,6 +66,9 @@ void fit<OBB>(Vec3f* ps, int n, OBB& bv);
template<>
void fit<RSS>(Vec3f* ps, int n, RSS& bv);
+template<>
+void fit<kIOS>(Vec3f* ps, int n, kIOS& bv);
+
template<typename BV>
class BVFitterBase
@@ -254,6 +258,52 @@ private:
BVHModelType type;
};
+
+/** \brief Specification of BVFitter for kIOS bounding volume */
+template<>
+class BVFitter<kIOS> : public BVFitterBase<kIOS>
+{
+public:
+ /** \brief Prepare the geometry primitive data for fitting */
+ void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
+ {
+ vertices = vertices_;
+ prev_vertices = NULL;
+ tri_indices = tri_indices_;
+ type = type_;
+ }
+
+ /** \brief Prepare the geometry primitive data for fitting */
+ void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
+ {
+ vertices = vertices_;
+ prev_vertices = prev_vertices_;
+ tri_indices = tri_indices_;
+ type = type_;
+ }
+
+ /** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
+ * The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
+ */
+ kIOS fit(unsigned int* primitive_indices, int num_primitives);
+
+ /** \brief Clear the geometry primitive data */
+ void clear()
+ {
+ vertices = NULL;
+ prev_vertices = NULL;
+ tri_indices = NULL;
+ type = BVH_MODEL_UNKNOWN;
+ }
+
+private:
+ Vec3f* vertices;
+ Vec3f* prev_vertices;
+ Triangle* tri_indices;
+ BVHModelType type;
+};
+
+
}
#endif
diff --git a/trunk/fcl/include/fcl/BV_splitter.h b/trunk/fcl/include/fcl/BV_splitter.h
index bd7ccabdfd5615fb5221da0972055608a9193721..fc1b46bf8c6a3db433421ecf00401dd7f3b24819 100644
--- a/trunk/fcl/include/fcl/BV_splitter.h
+++ b/trunk/fcl/include/fcl/BV_splitter.h
@@ -42,6 +42,8 @@
#include "fcl/primitive.h"
#include "fcl/vec_3f.h"
#include "fcl/OBB.h"
+#include "fcl/RSS.h"
+#include "fcl/kIOS.h"
#include <vector>
#include <iostream>
@@ -302,6 +304,153 @@ private:
SplitMethodType split_method;
};
+
+/** \brief BVHSplitRule specialization for RSS */
+template<>
+class BVSplitter<RSS> : public BVSplitterBase<RSS>
+{
+public:
+
+ BVSplitter(SplitMethodType method)
+ {
+ split_method = method;
+ }
+
+ /** \brief Set the geometry data needed by the split rule */
+ void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
+ {
+ vertices = vertices_;
+ tri_indices = tri_indices_;
+ type = type_;
+ }
+
+ /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
+ void computeRule(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
+ {
+ switch(split_method)
+ {
+ case SPLIT_METHOD_MEAN:
+ computeRule_mean(bv, primitive_indices, num_primitives);
+ break;
+ case SPLIT_METHOD_MEDIAN:
+ computeRule_median(bv, primitive_indices, num_primitives);
+ break;
+ case SPLIT_METHOD_BV_CENTER:
+ computeRule_bvcenter(bv, primitive_indices, num_primitives);
+ break;
+ default:
+ std::cerr << "Split method not supported" << std::endl;
+ }
+ }
+
+ /** \brief Apply the split rule on a given point */
+ bool apply(const Vec3f& q) const
+ {
+ return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+ }
+
+ /** \brief Clear the geometry data set before */
+ void clear()
+ {
+ vertices = NULL;
+ tri_indices = NULL;
+ type = BVH_MODEL_UNKNOWN;
+ }
+
+private:
+
+ /** \brief Split the node from center */
+ void computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+ /** \brief Split the node according to the mean of the data contained */
+ void computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+ /** \brief Split the node according to the median of the data contained */
+ void computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
+
+ BVH_REAL split_value;
+ Vec3f split_vector;
+
+ Vec3f* vertices;
+ Triangle* tri_indices;
+ BVHModelType type;
+ SplitMethodType split_method;
+};
+
+
+/** \brief BVHSplitRule specialization for RSS */
+template<>
+class BVSplitter<kIOS> : public BVSplitterBase<kIOS>
+{
+public:
+
+ BVSplitter(SplitMethodType method)
+ {
+ split_method = method;
+ }
+
+ /** \brief Set the geometry data needed by the split rule */
+ void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
+ {
+ vertices = vertices_;
+ tri_indices = tri_indices_;
+ type = type_;
+ }
+
+ /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
+ void computeRule(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
+ {
+ switch(split_method)
+ {
+ case SPLIT_METHOD_MEAN:
+ computeRule_mean(bv, primitive_indices, num_primitives);
+ break;
+ case SPLIT_METHOD_MEDIAN:
+ computeRule_median(bv, primitive_indices, num_primitives);
+ break;
+ case SPLIT_METHOD_BV_CENTER:
+ computeRule_bvcenter(bv, primitive_indices, num_primitives);
+ break;
+ default:
+ std::cerr << "Split method not supported" << std::endl;
+ }
+ }
+
+ /** \brief Apply the split rule on a given point */
+ bool apply(const Vec3f& q) const
+ {
+ return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
+ }
+
+ /** \brief Clear the geometry data set before */
+ void clear()
+ {
+ vertices = NULL;
+ tri_indices = NULL;
+ type = BVH_MODEL_UNKNOWN;
+ }
+
+private:
+
+ /** \brief Split the node from center */
+ void computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
+
+ /** \brief Split the node according to the mean of the data contained */
+ void computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
+
+ /** \brief Split the node according to the median of the data contained */
+ void computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
+
+ BVH_REAL split_value;
+ Vec3f split_vector;
+
+ Vec3f* vertices;
+ Triangle* tri_indices;
+ BVHModelType type;
+ SplitMethodType split_method;
+};
+
+
}
#endif
diff --git a/trunk/fcl/include/fcl/OBB.h b/trunk/fcl/include/fcl/OBB.h
index 2d03d8c756112adb91823a10816ef4cc843fcc35..c4698151f8a30e3ae068efc129d0c26040c84aa3 100644
--- a/trunk/fcl/include/fcl/OBB.h
+++ b/trunk/fcl/include/fcl/OBB.h
@@ -118,7 +118,7 @@ public:
}
/** \brief Center of the OBB */
- inline Vec3f center() const
+ inline const Vec3f& center() const
{
return To;
}
diff --git a/trunk/fcl/include/fcl/RSS.h b/trunk/fcl/include/fcl/RSS.h
index 61c435731928ed552cc95e56bb5e44dd82a1839a..83cf4281f4c9146312012d769354944e15153f3e 100644
--- a/trunk/fcl/include/fcl/RSS.h
+++ b/trunk/fcl/include/fcl/RSS.h
@@ -121,7 +121,7 @@ public:
}
/** \brief The RSS center */
- inline Vec3f center() const
+ inline const Vec3f& center() const
{
return Tr;
}
diff --git a/trunk/fcl/include/fcl/collision_node.h b/trunk/fcl/include/fcl/collision_node.h
index 29f8a46cf585ea71aab0b747a8a26229e2e4a3b6..054a963e1c6faab0e60d773bc0286953c0c156a2 100644
--- a/trunk/fcl/include/fcl/collision_node.h
+++ b/trunk/fcl/include/fcl/collision_node.h
@@ -57,6 +57,8 @@ void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL,
void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list = NULL, int qsize = 2);
+void distance(MeshDistanceTraversalNodekIOS* node, BVHFrontList* front_list = NULL, int qsize = 2);
+
}
#endif
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index bb9da734ce11b3ed224fe98713aab7f636b082bc..d43893bcfbf4639f705a7f439fb0b50b7f63ce25 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -47,7 +47,7 @@ namespace fcl
enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
-enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
+enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KIOS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE};
class CollisionGeometry
diff --git a/trunk/fcl/include/fcl/kIOS.h b/trunk/fcl/include/fcl/kIOS.h
new file mode 100644
index 0000000000000000000000000000000000000000..198d98e009a08cbfc36fae12d886a3bcffcb3452
--- /dev/null
+++ b/trunk/fcl/include/fcl/kIOS.h
@@ -0,0 +1,151 @@
+/*
+ * 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_KIOS_H
+#define FCL_KIOS_H
+
+#include "fcl/BVH_internal.h"
+#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
+
+/** \brief Main namespace */
+namespace fcl
+{
+
+/** \brief kIOS class */
+class kIOS
+{
+ struct kIOS_Sphere
+ {
+ Vec3f o;
+ BVH_REAL r;
+ };
+
+ static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
+ {
+ Vec3f d = s1.o - s0.o;
+ BVH_REAL dist2 = d.sqrLength();
+ BVH_REAL diff_r = s1.r - s0.r;
+
+ /** The sphere with the larger radius encloses the other */
+ if(diff_r * diff_r >= dist2)
+ {
+ if(s1.r > s0.r) return s1;
+ else return s0;
+ }
+ else /** spheres partially overlapping or disjoint */
+ {
+ float dist = sqrt(dist2);
+ kIOS_Sphere s;
+ s.r = dist + s0.r + s1.r;
+ s.o = s0.o;
+ if(dist > 0)
+ s.o += d * ((s.r - s0.r) / dist);
+
+ return s;
+ }
+ }
+public:
+
+ /** \brief The (at most) three spheres for intersection */
+ kIOS_Sphere spheres[5];
+
+ unsigned int num_spheres; // num_spheres <= 5
+
+ kIOS() {}
+
+ /** \brief Check collision between two kIOS */
+ bool overlap(const kIOS& other) const;
+
+ /** \brief Check collision between two kIOS and return the overlap part.
+ * For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
+ */
+ bool overlap(const kIOS& other, kIOS& overlap_part) const
+ {
+ return overlap(other);
+ }
+
+ /** \brief Check whether the kIOS contains a point */
+ inline bool contain(const Vec3f& p) const;
+
+ /** \brief A simple way to merge the kIOS and a point */
+ kIOS& operator += (const Vec3f& p);
+
+ /** \brief Merge the kIOS and another kIOS */
+ kIOS& operator += (const kIOS& other)
+ {
+ *this = *this + other;
+ return *this;
+ }
+
+ /** \brief Return the merged kIOS of current kIOS and the other one */
+ kIOS operator + (const kIOS& other) const;
+
+ /** \brief Center of the kIOS */
+ const Vec3f& center() const
+ {
+ return spheres[0].o;
+ }
+
+ /** \brief width of the kIOS */
+ BVH_REAL width() const;
+
+ /** \brief height of the kIOS */
+ BVH_REAL height() const;
+
+ /** \brief depth of the kIOS */
+ BVH_REAL depth() const;
+
+ /** \brief volume of the kIOS */
+ BVH_REAL volume() const;
+
+ /** \brief size of the kIOS, for split order */
+ BVH_REAL size() const;
+
+ /** \brief The distance between two kIOS */
+ BVH_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
+
+private:
+
+};
+
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
+
+BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 801554c2b5c77d4ce6f9db9d25bfbacc2a5e287c..c09d1aec0115ee34442c1f988813fe05bd928771 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -279,6 +279,10 @@ bool initialize(MeshCollisionTraversalNodeRSS& node,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
+bool initialize(MeshCollisionTraversalNodekIOS& node,
+ const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
+ const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
#if USE_SVMLIGHT
@@ -544,6 +548,11 @@ bool initialize(MeshDistanceTraversalNodeRSS& node,
const BVHModel<RSS>& model2, const SimpleTransform& tf2);
+bool initialize(MeshDistanceTraversalNodekIOS& node,
+ const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
+ const BVHModel<kIOS>& model2, const SimpleTransform& tf2);
+
+
/** \brief Initialize traversal node for continuous collision detection between two meshes */
template<typename BV>
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index c2a5bc0ec26b6c958684a1bfb70f35ef5253c7be..3ba605f3fce813bbb7bf25ac8905011561b2d2e0 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -301,7 +301,18 @@ public:
Vec3f T;
};
+class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
+{
+public:
+ MeshCollisionTraversalNodekIOS();
+
+ bool BVTesting(int b1, int b2) const;
+
+ void leafTesting(int b1, int b2) const;
+ Matrix3f R;
+ Vec3f T;
+};
#if USE_SVMLIGHT
@@ -1048,6 +1059,19 @@ public:
};
+class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode<kIOS>
+{
+public:
+ MeshDistanceTraversalNodekIOS();
+
+ BVH_REAL BVTesting(int b1, int b2) const;
+
+ void leafTesting(int b1, int b2) const;
+
+ Matrix3f R;
+ Vec3f T;
+};
+
struct ConservativeAdvancementStackData
{
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH_model.cpp
index cec7809d913c1515325301bff193382f7bccdb15..da0e4c2a87428e3c26c48e2de7c9fe7197f1f290 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH_model.cpp
@@ -891,6 +891,12 @@ NODE_TYPE BVHModel<RSS>::getNodeType() const
return BV_RSS;
}
+template<>
+NODE_TYPE BVHModel<kIOS>::getNodeType() const
+{
+ return BV_KIOS;
+}
+
template<>
NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const
{
@@ -917,5 +923,5 @@ template class BVHModel<KDOP<24> >;
template class BVHModel<OBB>;
template class BVHModel<AABB>;
template class BVHModel<RSS>;
-
+template class BVHModel<kIOS>;
}
diff --git a/trunk/fcl/src/BVH_utility.cpp b/trunk/fcl/src/BVH_utility.cpp
index 00a4ce2f458be0f006c148a2660ccfa8185f5bdd..c506784a09de20e3d643f5065d28553c736f2ea2 100644
--- a/trunk/fcl/src/BVH_utility.cpp
+++ b/trunk/fcl/src/BVH_utility.cpp
@@ -1026,6 +1026,81 @@ void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indic
}
+void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, BVH_REAL& radius)
+{
+ Vec3f e1 = a - c;
+ Vec3f e2 = b - c;
+ BVH_REAL e1_len2 = e1.sqrLength();
+ BVH_REAL e2_len2 = e2.sqrLength();
+ Vec3f e3 = e1.cross(e2);
+ BVH_REAL e3_len2 = e3.sqrLength();
+ radius = e1_len2 * e2_len2 * (e1 - e2).sqrLength() / e3_len2;
+ radius = sqrt(radius) / 2.0;
+
+ center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
+}
+
+
+BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
+{
+ bool indirect_index = true;
+ if(!indices) indirect_index = false;
+
+ BVH_REAL maxD = 0;
+ for(int i = 0; i < n; ++i)
+ {
+ unsigned int index = indirect_index ? indices[i] : i;
+ const Triangle& t = ts[index];
+
+ for(int j = 0; j < 3; ++j)
+ {
+ int point_id = t[j];
+ const Vec3f& p = ps[point_id];
+
+ BVH_REAL d = (p - query).sqrLength();
+ if(d > maxD) maxD = d;
+ }
+
+ if(ps2)
+ {
+ for(int j = 0; j < 3; ++j)
+ {
+ int point_id = t[j];
+ const Vec3f& p = ps2[point_id];
+
+ BVH_REAL d = (p - query).sqrLength();
+ if(d > maxD) maxD = d;
+ }
+ }
+ }
+
+ return sqrt(maxD);
+}
+
+BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query)
+{
+ bool indirect_index = true;
+ if(!indices) indirect_index = false;
+
+ BVH_REAL maxD = 0;
+ for(unsigned int i = 0; i < n; ++i)
+ {
+ int index = indirect_index ? indices[i] : i;
+
+ const Vec3f& p = ps[index];
+ BVH_REAL d = (p - query).sqrLength();
+ if(d > maxD) maxD = d;
+
+ if(ps2)
+ {
+ const Vec3f& v = ps2[index];
+ BVH_REAL d = (v - query).sqrLength();
+ if(d > maxD) maxD = d;
+ }
+ }
+
+ return sqrt(maxD);
+}
}
diff --git a/trunk/fcl/src/BV_fitter.cpp b/trunk/fcl/src/BV_fitter.cpp
index 2fe1bd5f93c1c6775612a61506a30a023e3618ea..8d0b1c805571ccf177371530e8608db2f1f5c0bc 100644
--- a/trunk/fcl/src/BV_fitter.cpp
+++ b/trunk/fcl/src/BV_fitter.cpp
@@ -37,6 +37,7 @@
#include "fcl/BV_fitter.h"
#include "fcl/BVH_utility.h"
#include <limits>
+#include <boost/math/constants/constants.hpp>
namespace fcl
{
@@ -56,8 +57,8 @@ void fit1(Vec3f* ps, OBB& bv)
void fit2(Vec3f* ps, OBB& bv)
{
- Vec3f p1(ps[0][0], ps[0][1], ps[0][2]);
- Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
+ const Vec3f& p1 = ps[0];
+ const Vec3f& p2 = ps[1];
Vec3f p1p2 = p1 - p2;
BVH_REAL len_p1p2 = p1p2.length();
p1p2.normalize();
@@ -74,9 +75,9 @@ void fit2(Vec3f* ps, OBB& bv)
void fit3(Vec3f* ps, OBB& bv)
{
- Vec3f p1(ps[0][0], ps[0][1], ps[0][2]);
- Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
- Vec3f p3(ps[2][0], ps[2][1], ps[2][2]);
+ const Vec3f& p1 = ps[0];
+ const Vec3f& p2 = ps[1];
+ const Vec3f& p3 = ps[2];
Vec3f e[3];
e[0] = p1 - p2;
e[1] = p2 - p3;
@@ -160,8 +161,8 @@ void fit1(Vec3f* ps, RSS& bv)
void fit2(Vec3f* ps, RSS& bv)
{
- Vec3f p1(ps[0][0], ps[0][1], ps[0][2]);
- Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
+ const Vec3f& p1 = ps[0];
+ const Vec3f& p2 = ps[1];
Vec3f p1p2 = p1 - p2;
BVH_REAL len_p1p2 = p1p2.length();
p1p2.normalize();
@@ -177,9 +178,9 @@ void fit2(Vec3f* ps, RSS& bv)
void fit3(Vec3f* ps, RSS& bv)
{
- Vec3f p1(ps[0][0], ps[0][1], ps[0][2]);
- Vec3f p2(ps[1][0], ps[1][1], ps[1][2]);
- Vec3f p3(ps[2][0], ps[2][1], ps[2][2]);
+ const Vec3f& p1 = ps[0];
+ const Vec3f& p2 = ps[1];
+ const Vec3f& p3 = ps[2];
Vec3f e[3];
e[0] = p1 - p2;
e[1] = p2 - p3;
@@ -239,8 +240,190 @@ void fitn(Vec3f* ps, int n, RSS& bv)
// set rss origin, rectangle size and radius
getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, n, bv.axis, bv.Tr, bv.l, bv.r);
}
+
+}
+
+namespace kIOS_fit_functions
+{
+
+void fit1(Vec3f* ps, kIOS& bv)
+{
+ bv.num_spheres = 1;
+ bv.spheres[0].o = ps[0];
+ bv.spheres[0].r = 0;
}
+void fit2(Vec3f* ps, kIOS& bv)
+{
+ bv.num_spheres = 5;
+
+ const Vec3f& p1 = ps[0];
+ const Vec3f& p2 = ps[1];
+ Vec3f p1p2 = p1 - p2;
+ BVH_REAL len_p1p2 = p1p2.length();
+ p1p2.normalize();
+
+ Vec3f axis[3];
+ axis[0] = p1p2;
+ generateCoordinateSystem(axis[0], axis[1], axis[2]);
+
+ BVH_REAL r0 = len_p1p2 / 2.0;
+ BVH_REAL r0cos30 = r0 * cos(boost::math::constants::pi<BVH_REAL>() / 6);
+ bv.spheres[0].o = (p1 + p2) * 0.5;
+ bv.spheres[0].r = r0;
+
+ BVH_REAL twor0 = 2 * r0;
+
+ bv.spheres[1].r = twor0;
+ bv.spheres[2].r = twor0;
+ Vec3f delta = axis[1] * r0cos30;
+ bv.spheres[1].o = bv.spheres[0].o - delta;
+ bv.spheres[2].o = bv.spheres[0].o + delta;
+
+ bv.spheres[3].r = twor0;
+ bv.spheres[4].r = twor0;
+ delta = axis[2] * r0cos30;
+ bv.spheres[3].o = bv.spheres[0].o - delta;
+ bv.spheres[4].o = bv.spheres[0].o + delta;
+}
+
+void fit3(Vec3f* ps, kIOS& bv)
+{
+ bv.num_spheres = 3;
+
+ const Vec3f& p1 = ps[0];
+ const Vec3f& p2 = ps[1];
+ const Vec3f& p3 = ps[2];
+ Vec3f e[3];
+ e[0] = p1 - p2;
+ e[1] = p2 - p3;
+ e[2] = p3 - p1;
+ BVH_REAL len[3];
+ len[0] = e[0].sqrLength();
+ len[1] = e[1].sqrLength();
+ len[2] = e[2].sqrLength();
+
+ int imax = 0;
+ if(len[1] > len[0]) imax = 1;
+ if(len[2] > len[1]) imax = 2;
+
+ Vec3f axis[3];
+
+ axis[2] = e[0].cross(e[1]);
+ axis[2].normalize();
+ axis[0] = e[imax];
+ axis[0].normalize();
+ axis[1] = axis[2].cross(axis[0]);
+
+
+ // compute radius and center
+ BVH_REAL r0;
+ Vec3f center;
+ circumCircleComputation(p1, p2, p3, center, r0);
+
+ bv.spheres[0].o = center;
+ bv.spheres[0].r = r0;
+
+ BVH_REAL twor0 = 2 * r0;
+ Vec3f delta = axis[2] * r0 * cos(boost::math::constants::pi<BVH_REAL>() / 6);
+
+ bv.spheres[1].r = twor0;
+ bv.spheres[1].o = center - delta;
+ bv.spheres[2].r = twor0;
+ bv.spheres[2].o = center + delta;
+}
+
+void fit6(Vec3f* ps, kIOS& bv)
+{
+ kIOS bv1, bv2;
+ fit3(ps, bv1);
+ fit3(ps + 3, bv2);
+ bv = bv1 + bv2;
+}
+
+void fitn(Vec3f* ps, int n, kIOS& bv)
+{
+ Matrix3f M;
+ Vec3f E[3];
+ BVH_REAL s[3] = {0, 0, 0}; // three eigen values;
+
+ getCovariance(ps, NULL, NULL, n, M);
+ matEigen(M, s, E);
+
+ int min, mid, max;
+ if(s[0] > s[1]) { max = 0; min = 1; }
+ else { min = 0; max = 1; }
+ if(s[2] < s[min]) { mid = min; min = 2; }
+ else if(s[2] > s[max]) { mid = max; max = 2; }
+ else { mid = 2; }
+
+ Vec3f axis[3];
+ axis[0].setValue(E[0][max], E[1][max], E[2][max]);
+ axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
+ axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
+ E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
+ E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
+
+ // get center and extension
+ Vec3f center, extent;
+ getExtentAndCenter(ps, NULL, NULL, n, axis, center, extent);
+
+ BVH_REAL r0 = maximumDistance(ps, NULL, NULL, n, center);
+
+ // decide the k in kIOS
+ if(extent[0] > 1.5 * extent[2])
+ {
+ if(extent[0] > 1.5 * extent[1]) bv.num_spheres = 5;
+ else bv.num_spheres = 3;
+ }
+ else bv.num_spheres = 1;
+
+ if(bv.num_spheres >= 1)
+ {
+ bv.spheres[0].o = center;
+ bv.spheres[0].r = r0;
+ }
+
+ BVH_REAL cos30 = cos(boost::math::constants::pi<BVH_REAL>() / 6);
+
+ if(bv.num_spheres >= 3)
+ {
+ BVH_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * 2;
+ Vec3f delta = axis[2] * (r10 * cos30 - extent[2]);
+ bv.spheres[1].o = center - delta;
+ bv.spheres[2].o = center + delta;
+
+ BVH_REAL r11 = 0, r12 = 0;
+ r11 = maximumDistance(ps, NULL, NULL, n, bv.spheres[1].o);
+ r12 = maximumDistance(ps, NULL, NULL, n, bv.spheres[2].o);
+ bv.spheres[1].o += axis[2] * (-r10 + r11);
+ bv.spheres[2].o += axis[2] * (r10 - r12);
+
+ bv.spheres[1].r = r10;
+ bv.spheres[2].r = r10;
+ }
+
+ if(bv.num_spheres >= 5)
+ {
+ BVH_REAL r20 = sqrt(r0 * r0 - extent[1] * extent[1]) * 2;
+ Vec3f delta = axis[1] * (r20 * cos30 - extent[1]);
+ bv.spheres[3].o = center - delta;
+ bv.spheres[4].o = center + delta;
+
+ BVH_REAL r21 = 0, r22 = 0;
+ r21 = maximumDistance(ps, NULL, NULL, n, bv.spheres[3].o);
+ r22 = maximumDistance(ps, NULL, NULL, n, bv.spheres[4].o);
+ bv.spheres[3].o += axis[3] * (-r20 + r21);
+ bv.spheres[4].o += axis[3] * (r20 - r22);
+
+ bv.spheres[3].r = r20;
+ bv.spheres[4].r = r20;
+ }
+}
+
+}
+
+
template<>
void fit(Vec3f* ps, int n, OBB& bv)
@@ -284,6 +467,28 @@ void fit(Vec3f* ps, int n, RSS& bv)
}
}
+template<>
+void fit(Vec3f* ps, int n, kIOS& bv)
+{
+ switch(n)
+ {
+ case 1:
+ kIOS_fit_functions::fit1(ps, bv);
+ break;
+ case 2:
+ kIOS_fit_functions::fit2(ps, bv);
+ break;
+ case 3:
+ kIOS_fit_functions::fit3(ps, bv);
+ break;
+ case 6:
+ kIOS_fit_functions::fit6(ps, bv);
+ break;
+ default:
+ kIOS_fit_functions::fitn(ps, n, bv);
+ }
+}
+
OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
{
@@ -334,6 +539,125 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
return bv;
}
+kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
+{
+ kIOS bv;
+
+ Matrix3f M; // row first matrix
+ Vec3f E[3]; // row first eigen-vectors
+ BVH_REAL s[3];
+
+ if(type == BVH_MODEL_TRIANGLES)
+ {
+ getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
+ matEigen(M, s, E);
+ }
+ else if(type == BVH_MODEL_POINTCLOUD)
+ {
+ getCovariance(vertices, prev_vertices, primitive_indices, num_primitives, M);
+ matEigen(M, s, E);
+ }
+
+ int min, mid, max;
+ if(s[0] > s[1]) { max = 0; min = 1; }
+ else { min = 0; max = 1; }
+ if(s[2] < s[min]) { mid = min; min = 2; }
+ else if(s[2] > s[max]) { mid = max; max = 2; }
+ else mid = 2;
+
+ Vec3f axis[3];
+ axis[0].setValue(E[0][max], E[1][max], E[2][max]);
+ axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
+ axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
+ E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
+ E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
+
+ // get centers and extensions
+ Vec3f center, extent;
+ if(type == BVH_MODEL_TRIANGLES)
+ {
+ getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axis, center, extent);
+ }
+ else if(type == BVH_MODEL_POINTCLOUD)
+ {
+ getExtentAndCenter(vertices, prev_vertices, primitive_indices, num_primitives, axis, center, extent);
+ }
+
+ BVH_REAL r0;
+ if(type == BVH_MODEL_TRIANGLES)
+ r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center);
+ else if(type == BVH_MODEL_POINTCLOUD)
+ r0 = maximumDistance(vertices, prev_vertices, primitive_indices, num_primitives, center);
+
+ // decide k in kIOS
+ if(extent[0] > 1.5 * extent[2])
+ {
+ if(extent[0] > 1.5 * extent[1]) bv.num_spheres = 5;
+ else bv.num_spheres = 3;
+ }
+ else bv.num_spheres = 1;
+
+ if(bv.num_spheres >= 1)
+ {
+ bv.spheres[0].o = center;
+ bv.spheres[0].r = r0;
+ }
+
+ BVH_REAL cos30 = cos(boost::math::constants::pi<BVH_REAL>() / 6);
+
+ if(bv.num_spheres >= 3)
+ {
+ BVH_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * 2;
+ Vec3f delta = axis[2] * (r10 * cos30 - extent[2]);
+ bv.spheres[1].o = center - delta;
+ bv.spheres[2].o = center + delta;
+
+ BVH_REAL r11 = 0, r12 = 0;
+ if(type == BVH_MODEL_TRIANGLES)
+ {
+ r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o);
+ r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o);
+ }
+ else if(type == BVH_MODEL_POINTCLOUD)
+ {
+ r11 = maximumDistance(vertices, prev_vertices, primitive_indices, num_primitives, bv.spheres[1].o);
+ r12 = maximumDistance(vertices, prev_vertices, primitive_indices, num_primitives, bv.spheres[2].o);
+ }
+ bv.spheres[1].o += axis[2] * (-r10 + r11);
+ bv.spheres[2].o += axis[2] * (r10 - r12);
+
+ bv.spheres[1].r = r10;
+ bv.spheres[2].r = r10;
+ }
+
+ if(bv.num_spheres >= 5)
+ {
+ BVH_REAL r20 = sqrt(r0 * r0 - extent[1] * extent[1]) * 2;
+ Vec3f delta = axis[1] * (r20 * cos30 - extent[1]);
+ bv.spheres[3].o = center - delta;
+ bv.spheres[4].o = center + delta;
+
+ BVH_REAL r21 = 0, r22 = 0;
+ if(type == BVH_MODEL_TRIANGLES)
+ {
+ r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o);
+ r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o);
+ }
+ else if(type == BVH_MODEL_POINTCLOUD)
+ {
+ r21 = maximumDistance(vertices, prev_vertices, primitive_indices, num_primitives, bv.spheres[3].o);
+ r22 = maximumDistance(vertices, prev_vertices, primitive_indices, num_primitives, bv.spheres[4].o);
+ }
+ bv.spheres[3].o += axis[1] * (-r20 + r21);
+ bv.spheres[4].o += axis[1] * (r20 - r22);
+
+ bv.spheres[3].r = r20;
+ bv.spheres[4].r = r20;
+ }
+
+ return bv;
+}
+
RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
{
diff --git a/trunk/fcl/src/BV_splitter.cpp b/trunk/fcl/src/BV_splitter.cpp
index e99d3be94f48dba8aea050e8b677d3bbaf1ca3d6..9d7fa74d97f8475f398036743ab24fc72896ef37 100644
--- a/trunk/fcl/src/BV_splitter.cpp
+++ b/trunk/fcl/src/BV_splitter.cpp
@@ -39,16 +39,54 @@
namespace fcl
{
-void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
+
+template<typename BV>
+void computeSplitVector(const BV& bv, Vec3f& split_vector)
{
- Vec3f center = bv.center();
split_vector = bv.axis[0];
+}
+
+template<>
+void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector)
+{
+ switch(bv.num_spheres)
+ {
+ case 1:
+ split_vector = Vec3f(1, 0, 0);
+ break;
+ case 3:
+ {
+ Vec3f v[3];
+ v[0] = bv.spheres[1].o - bv.spheres[0].o;
+ v[0].normalize();
+ generateCoordinateSystem(v[0], v[1], v[2]);
+ split_vector = v[1];
+ }
+ break;
+ case 5:
+ {
+ Vec3f v[2];
+ v[0] = bv.spheres[1].o - bv.spheres[0].o;
+ v[1] = bv.spheres[3].o - bv.spheres[0].o;
+ split_vector = v[0].cross(v[1]);
+ split_vector.normalize();
+ }
+ break;
+ default:
+ ;
+ }
+}
+
+template<typename BV>
+void computeSplitValue_bvcenter(const BV& bv, BVH_REAL& split_value)
+{
+ Vec3f center = bv.center();
split_value = center[0];
}
-void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
+template<typename BV>
+void computeSplitValue_mean(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, BVH_REAL& split_value)
{
- split_vector = bv.axis[0];
BVH_REAL sum = 0.0;
if(type == BVH_MODEL_TRIANGLES)
{
@@ -56,7 +94,7 @@ void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_in
for(int i = 0; i < num_primitives; ++i)
{
- const Triangle& t = tri_indices[primitive_indices[i]];
+ const Triangle& t = triangles[primitive_indices[i]];
const Vec3f& p1 = vertices[t[0]];
const Vec3f& p2 = vertices[t[1]];
const Vec3f& p3 = vertices[t[2]];
@@ -80,17 +118,16 @@ void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_in
}
}
-
-void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
+template<typename BV>
+void computeSplitValue_median(const BV& bv, Vec3f* vertices, Triangle* triangles, unsigned int* primitive_indices, int num_primitives, BVHModelType type, const Vec3f& split_vector, BVH_REAL& split_value)
{
- split_vector = bv.axis[0];
std::vector<BVH_REAL> proj(num_primitives);
if(type == BVH_MODEL_TRIANGLES)
{
for(int i = 0; i < num_primitives; ++i)
{
- const Triangle& t = tri_indices[primitive_indices[i]];
+ const Triangle& t = triangles[primitive_indices[i]];
const Vec3f& p1 = vertices[t[0]];
const Vec3f& p2 = vertices[t[1]];
const Vec3f& p3 = vertices[t[2]];
@@ -120,7 +157,64 @@ void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_
else
{
split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
- }
+ }
+}
+
+
+void BVSplitter<OBB>::computeRule_bvcenter(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
+{
+ computeSplitVector<OBB>(bv, split_vector);
+ computeSplitValue_bvcenter<OBB>(bv, split_value);
}
+void BVSplitter<OBB>::computeRule_mean(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
+{
+ computeSplitVector<OBB>(bv, split_vector);
+ computeSplitValue_mean<OBB>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
+}
+
+void BVSplitter<OBB>::computeRule_median(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
+{
+ computeSplitVector<OBB>(bv, split_vector);
+ computeSplitValue_median<OBB>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
+}
+
+
+void BVSplitter<RSS>::computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
+{
+ computeSplitVector<RSS>(bv, split_vector);
+ computeSplitValue_bvcenter<RSS>(bv, split_value);
+}
+
+void BVSplitter<RSS>::computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
+{
+ computeSplitVector<RSS>(bv, split_vector);
+ computeSplitValue_mean<RSS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
+}
+
+void BVSplitter<RSS>::computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
+{
+ computeSplitVector<RSS>(bv, split_vector);
+ computeSplitValue_median<RSS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
+}
+
+void BVSplitter<kIOS>::computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
+{
+ computeSplitVector<kIOS>(bv, split_vector);
+ computeSplitValue_bvcenter<kIOS>(bv, split_value);
+}
+
+void BVSplitter<kIOS>::computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
+{
+ computeSplitVector<kIOS>(bv, split_vector);
+ computeSplitValue_mean<kIOS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
+}
+
+void BVSplitter<kIOS>::computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
+{
+ computeSplitVector<kIOS>(bv, split_vector);
+ computeSplitValue_median<kIOS>(bv, vertices, tri_indices, primitive_indices, num_primitives, type, split_vector, split_value);
+}
+
+
}
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 709057da4f2e1c1a0448150debcfe26ea5f9c243..1ad46a1c37827fdef9fc60d68974af9138350848 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -48,1008 +48,219 @@
namespace fcl
{
-/** \brief Hey, I know it is ugly... but it is the best way I can find now... */
-
-#define SHAPESHAPE_COMMON_CODE() do{ \
- initialize(node, *obj1, tf1, *obj2, tf2, enable_contact); \
- collide(&node); \
- if(!node.is_collision) return 0; \
- contacts.resize(1); \
- if(!enable_contact) contacts[0] = Contact(o1, o2, 0, 0); \
- else contacts[0] = Contact(o1, o2, 0, 0, node.contact_point, node.normal, node.penetration_depth); \
- return 1; \
- } while(0)
-
-
-#define MESHSHAPE_COMMON_CODE() do{ \
- initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, num_max_contacts, exhaustive, enable_contact); \
- collide(&node); \
- int num_contacts = node.pairs.size(); \
- if(num_contacts > 0) \
- { \
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
- contacts.resize(num_contacts); \
- if(!enable_contact) \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0); \
- } \
- else \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0, node.pairs[i].contact_point, node.pairs[i].normal, node.pairs[i].penetration_depth); \
- } \
- } \
- delete obj1_tmp; \
- obj1_tmp = NULL; \
- return num_contacts; \
- } while(0)
-
-
-#define SHAPEMESH_COMMON_CODE() do{ \
- initialize(node, *obj1, tf1, *obj2_tmp, tf2_tmp, num_max_contacts, exhaustive, enable_contact); \
- collide(&node); \
- int num_contacts = node.pairs.size(); \
- if(num_contacts > 0) \
- { \
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
- contacts.resize(num_contacts); \
- if(!enable_contact) \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0); \
- } \
- else \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0, node.pairs[i].contact_point, node.pairs[i].normal, node.pairs[i].penetration_depth); \
- } \
- } \
- delete obj2_tmp; \
- obj2_tmp = NULL; \
- return num_contacts; \
- } while(0)
-
-#define MESHSHAPEOBBRSS_COMMON_CODE() do{ \
- initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact); \
- collide(&node); \
- int num_contacts = node.pairs.size(); \
- if(num_contacts > 0) \
- { \
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
- contacts.resize(num_contacts); \
- if(!enable_contact) \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0); \
- } \
- else \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id, 0, node.pairs[i].contact_point, node.pairs[i].normal, node.pairs[i].penetration_depth); \
- } \
- } \
- return num_contacts; \
- } while(0)
-
-#define MESHMESH_COMMON_CODE() do{ \
- initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, num_max_contacts, exhaustive, enable_contact); \
- collide(&node); \
- int num_contacts = node.pairs.size(); \
- if(num_contacts > 0) \
- { \
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
- contacts.resize(num_contacts); \
- if(!enable_contact) \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2); \
- } \
- else \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2, node.pairs[i].contact_point, node.pairs[i].normal, node.pairs[i].penetration_depth); \
- } \
- } \
- delete obj1_tmp; \
- obj1_tmp = NULL; \
- delete obj2_tmp; \
- obj2_tmp = NULL; \
- return num_contacts; \
- } while(0)
-
-
-#define MESHMESHOBBRSS_COMMON_CODE() do{ \
- initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact); \
- collide(&node); \
- int num_contacts = node.pairs.size(); \
- if(num_contacts > 0) \
- { \
- if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts; \
- contacts.resize(num_contacts); \
- if(!enable_contact) \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2); \
- } \
- else \
- { \
- for(int i = 0; i < num_contacts; ++i) \
- { \
- Vec3f normal = tf1.getRotation() * node.pairs[i].normal; \
- Vec3f contact_point = tf1.transform(node.pairs[i].contact_point); \
- contacts[i] = Contact(obj1, obj2, node.pairs[i].id1, node.pairs[i].id2, contact_point, normal, node.pairs[i].penetration_depth); \
- } \
- } \
- } \
- return num_contacts; \
- } while(0)
-
-
-
-int BoxBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<typename T_SH1, typename T_SH2>
+int ShapeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
- ShapeCollisionTraversalNode<Box, Box> node;
- const Box* obj1 = (Box*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
+ ShapeCollisionTraversalNode<T_SH1, T_SH2> node;
+ const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
+ const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
+ initialize(node, *obj1, tf1, *obj2, tf2, enable_contact);
+ collide(&node);
+ if(!node.is_collision) return 0;
+ contacts.resize(1);
+ if(!enable_contact) contacts[0] = Contact(o1, o2, 0, 0);
+ else contacts[0] = Contact(o1, o2, 0, 0, node.contact_point, node.normal, node.penetration_depth);
+ return 1;
}
-int BoxSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Sphere> node;
- const Box* obj1 = (Box*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Capsule> node;
- const Box* obj1 = (Box*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Cone> node;
- const Box* obj1 = (Box*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Cylinder> node;
- const Box* obj1 = (Box*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Convex> node;
- const Box* obj1 = (Box*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int BoxPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Box, Plane> node;
- const Box* obj1 = (Box*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Box> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Sphere> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Capsule> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Cone> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Cylinder> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SphereConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Convex> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int SpherePlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Sphere, Plane> node;
- const Sphere* obj1 = (Sphere*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Box> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Sphere> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Capsule> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Cone> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Cylinder> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Convex> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CapPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Capsule, Plane> node;
- const Capsule* obj1 = (Capsule*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Box> node;
- const Cone* obj1 = (Cone*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Sphere> node;
- const Cone* obj1 = (Cone*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Capsule> node;
- const Cone* obj1 = (Cone*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Cone> node;
- const Cone* obj1 = (Cone*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConeCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Cylinder> node;
- const Cone* obj1 = (Cone*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-int ConeConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Convex> node;
- const Cone* obj1 = (Cone*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConePlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cone, Plane> node;
- const Cone* obj1 = (Cone*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Box> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Sphere> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Capsule> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Cone> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Cylinder> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Convex> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int CylinderPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Cylinder, Plane> node;
- const Cylinder* obj1 = (Cylinder*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Box> node;
- const Convex* obj1 = (Convex*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Sphere> node;
- const Convex* obj1 = (Convex*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Capsule> node;
- const Convex* obj1 = (Convex*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Cone> node;
- const Convex* obj1 = (Convex*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Cylinder> node;
- const Convex* obj1 = (Convex*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Convex> node;
- const Convex* obj1 = (Convex*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int ConvexPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Convex, Plane> node;
- const Convex* obj1 = (Convex*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Box> node;
- const Plane* obj1 = (Plane*)o1;
- const Box* obj2 = (Box*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Sphere> node;
- const Plane* obj1 = (Plane*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Capsule> node;
- const Plane* obj1 = (Plane*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-int PlaneConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<typename T_BVH, typename T_SH>
+int BVHShapeContactCollection(const std::vector<BVHShapeCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const T_SH* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
- ShapeCollisionTraversalNode<Plane, Cone> node;
- const Plane* obj1 = (Plane*)o1;
- const Cone* obj2 = (Cone*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Cylinder> node;
- const Plane* obj1 = (Plane*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlaneConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Convex> node;
- const Plane* obj1 = (Plane*)o1;
- const Convex* obj2 = (Convex*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int PlanePlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- ShapeCollisionTraversalNode<Plane, Plane> node;
- const Plane* obj1 = (Plane*)o1;
- const Plane* obj2 = (Plane*)o2;
- SHAPESHAPE_COMMON_CODE();
-}
-
-int AABBBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Box> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Box* obj2 = (Box*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Sphere> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Sphere* obj2 = (Sphere*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Capsule> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Capsule* obj2 = (Capsule*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Cone> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cone* obj2 = (Cone*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Cylinder> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cylinder* obj2 = (Cylinder*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Convex> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Convex* obj2 = (Convex*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int AABBPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<AABB, Plane> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Plane* obj2 = (Plane*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int OBBBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Box> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Box* obj2 = (Box*)o2;
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Sphere> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Sphere* obj2 = (Sphere*)o2;
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Capsule> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Capsule* obj2 = (Capsule*)o2;
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Cone> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Cone* obj2 = (Cone*)o2;
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Cylinder> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Cylinder* obj2 = (Cylinder*)o2;
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Convex> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Convex* obj2 = (Convex*)o2;
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int OBBPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNodeOBB<Plane> node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const Plane* obj2 = (Plane*)o2;
- MESHSHAPEOBBRSS_COMMON_CODE();
-}
-
-int RSSBoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Box> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Box* obj2 = (Box*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int RSSSphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Sphere> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Sphere* obj2 = (Sphere*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int RSSCapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Capsule> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Capsule* obj2 = (Capsule*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-
-int RSSConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Cone> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cone* obj2 = (Cone*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ int num_contacts = pairs.size();
+ if(num_contacts > 0)
+ {
+ if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
+ contacts.resize(num_contacts);
+ if(!enable_contact)
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].id, 0);
+ }
+ else
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].id, 0, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ }
+ }
-int RSSCylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Cylinder> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cylinder* obj2 = (Cylinder*)o2;
- MESHSHAPE_COMMON_CODE();
+ return num_contacts;
}
-int RSSConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<RSS, Convex> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Convex* obj2 = (Convex*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-int RSSPlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<typename T_BVH, typename T_SH>
+struct BVHShapeCollider
{
- MeshShapeCollisionTraversalNode<RSS, Plane> node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- BVHModel<RSS>* obj1_tmp = new BVHModel<RSS>(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Plane* obj2 = (Plane*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ static int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ {
+ MeshShapeCollisionTraversalNode<T_BVH, T_SH> node;
+ const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
+ BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
+ SimpleTransform tf1_tmp = tf1;
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
-int KDOP16BoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Box> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Box* obj2 = (Box*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ fcl::collide(&node);
-int KDOP16SphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Sphere> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Sphere* obj2 = (Sphere*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ int num_contacts = BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
-int KDOP16CapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Capsule> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Capsule* obj2 = (Capsule*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ delete obj1_tmp;
+ return num_contacts;
+ }
+};
-int KDOP16ConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Cone> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cone* obj2 = (Cone*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-int KDOP16CylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<typename T_SH>
+struct BVHShapeCollider<OBB, T_SH>
{
- MeshShapeCollisionTraversalNode<KDOP<16> , Cylinder> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cylinder* obj2 = (Cylinder*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ static int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ {
+ MeshShapeCollisionTraversalNodeOBB<T_SH> node;
+ const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
-int KDOP16ConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Convex> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Convex* obj2 = (Convex*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ fcl::collide(&node);
-int KDOP16PlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<16> , Plane> node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Plane* obj2 = (Plane*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ }
+};
-int KDOP18BoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+/*
+template<typename T_SH>
+struct BVHShapeCollider<RSS, T_SH>
{
- MeshShapeCollisionTraversalNode<KDOP<18> , Box> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Box* obj2 = (Box*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ static int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ {
+ MeshShapeCollisionTraversalNodeRSS<T_SH> node;
+ const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
-int KDOP18SphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Sphere> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Sphere* obj2 = (Sphere*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ fcl::collide(&node);
-int KDOP18CapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Capsule> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Capsule* obj2 = (Capsule*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ }
+};
-int KDOP18ConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Cone> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cone* obj2 = (Cone*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-int KDOP18CylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<typename T_SH>
+struct BVHShapeCollider<kIOS, T_SH>
{
- MeshShapeCollisionTraversalNode<KDOP<18> , Cylinder> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cylinder* obj2 = (Cylinder*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ static int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+ {
+ MeshShapeCollisionTraversalNodekIOS<T_SH> node;
+ const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
-int KDOP18ConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Convex> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Convex* obj2 = (Convex*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ fcl::collide(&node);
-int KDOP18PlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<18> , Plane> node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Plane* obj2 = (Plane*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ }
+};
-int KDOP24BoxCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Box> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Box* obj2 = (Box*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+*/
-int KDOP24SphereCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<typename T_BVH>
+int BVHContactCollection(const std::vector<BVHCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
- MeshShapeCollisionTraversalNode<KDOP<24> , Sphere> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Sphere* obj2 = (Sphere*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ int num_contacts = pairs.size();
+ if(num_contacts > 0)
+ {
+ if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
+ contacts.resize(num_contacts);
+ if(!enable_contact)
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2);
+ }
+ else
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2, pairs[i].contact_point, pairs[i].normal, pairs[i].penetration_depth);
+ }
+ }
-int KDOP24CapCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Capsule> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Capsule* obj2 = (Capsule*)o2;
- MESHSHAPE_COMMON_CODE();
+ return num_contacts;
}
-int KDOP24ConeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Cone> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cone* obj2 = (Cone*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-int KDOP24CylinderCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<typename T_BVH>
+int BVHContactCollection_OBB_and_RSS_and_kIOS(const std::vector<BVHCollisionPair>& pairs, const SimpleTransform& tf1, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
- MeshShapeCollisionTraversalNode<KDOP<24> , Cylinder> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Cylinder* obj2 = (Cylinder*)o2;
- MESHSHAPE_COMMON_CODE();
-}
+ int num_contacts = pairs.size();
+ if(num_contacts > 0)
+ {
+ if((!exhaustive) && (num_contacts > num_max_contacts)) num_contacts = num_max_contacts;
+ contacts.resize(num_contacts);
+ if(!enable_contact)
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2);
+ }
+ else
+ {
+ for(int i = 0; i < num_contacts; ++i)
+ {
+ Vec3f normal = tf1.getRotation() * pairs[i].normal;
+ Vec3f contact_point = tf1.transform(pairs[i].contact_point);
+ contacts[i] = Contact(obj1, obj2, pairs[i].id1, pairs[i].id2, contact_point, normal, pairs[i].penetration_depth);
+ }
+ }
+ }
-int KDOP24ConvexCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Convex> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Convex* obj2 = (Convex*)o2;
- MESHSHAPE_COMMON_CODE();
+ return num_contacts;
}
-int KDOP24PlaneCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshShapeCollisionTraversalNode<KDOP<24> , Plane> node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- const Plane* obj2 = (Plane*)o2;
- MESHSHAPE_COMMON_CODE();
-}
-int AABBAABBCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<typename T_BVH>
+int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
- MeshCollisionTraversalNode<AABB> node;
- const BVHModel<AABB>* obj1 = (BVHModel<AABB>*)o1;
- const BVHModel<AABB>* obj2 = (BVHModel<AABB>*)o2;
- BVHModel<AABB>* obj1_tmp = new BVHModel<AABB>(*obj1);
+ MeshCollisionTraversalNode<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);
+ BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
SimpleTransform tf1_tmp = tf1;
- BVHModel<AABB>* obj2_tmp = new BVHModel<AABB>(*obj2);
+ BVHModel<T_BVH>* obj2_tmp = new BVHModel<T_BVH>(*obj2);
SimpleTransform tf2_tmp = tf2;
- MESHMESH_COMMON_CODE();
+
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2_tmp, tf2_tmp, num_max_contacts, exhaustive, enable_contact);
+ collide(&node);
+ int num_contacts = BVHContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+
+ delete obj1_tmp;
+ delete obj2_tmp;
+ return num_contacts;
}
-int OBBOBBCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<>
+int BVHCollide<OBB>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
MeshCollisionTraversalNodeOBB node;
- const BVHModel<OBB>* obj1 = (BVHModel<OBB>*)o1;
- const BVHModel<OBB>* obj2 = (BVHModel<OBB>*)o2;
- MESHMESHOBBRSS_COMMON_CODE();
-}
+ const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
+ const BVHModel<OBB>* obj2 = static_cast<const BVHModel<OBB>* >(o2);
-int RSSRSSCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshCollisionTraversalNodeRSS node;
- const BVHModel<RSS>* obj1 = (BVHModel<RSS>*)o1;
- const BVHModel<RSS>* obj2 = (BVHModel<RSS>*)o2;
- MESHMESHOBBRSS_COMMON_CODE();
-}
+ initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ collide(&node);
-int KDOP16KDOP16Collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshCollisionTraversalNode<KDOP<16> > node;
- const BVHModel<KDOP<16> >* obj1 = (BVHModel<KDOP<16> >*)o1;
- const BVHModel<KDOP<16> >* obj2 = (BVHModel<KDOP<16> >*)o2;
- BVHModel<KDOP<16> >* obj1_tmp = new BVHModel<KDOP<16> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- BVHModel<KDOP<16> >* obj2_tmp = new BVHModel<KDOP<16> >(*obj2);
- SimpleTransform tf2_tmp = tf2;
- MESHMESH_COMMON_CODE();
+ return BVHContactCollection_OBB_and_RSS_and_kIOS(node.pairs, tf1, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
}
-int KDOP18KDOP18Collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+template<>
+int BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
{
- MeshCollisionTraversalNode<KDOP<18> > node;
- const BVHModel<KDOP<18> >* obj1 = (BVHModel<KDOP<18> >*)o1;
- const BVHModel<KDOP<18> >* obj2 = (BVHModel<KDOP<18> >*)o2;
- BVHModel<KDOP<18> >* obj1_tmp = new BVHModel<KDOP<18> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- BVHModel<KDOP<18> >* obj2_tmp = new BVHModel<KDOP<18> >(*obj2);
- SimpleTransform tf2_tmp = tf2;
- MESHMESH_COMMON_CODE();
-}
+ MeshCollisionTraversalNodekIOS node;
+ const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
+ const BVHModel<kIOS>* obj2 = static_cast<const BVHModel<kIOS>* >(o2);
-int KDOP24KDOP24Collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
-{
- MeshCollisionTraversalNode<KDOP<24> > node;
- const BVHModel<KDOP<24> >* obj1 = (BVHModel<KDOP<24> >*)o1;
- const BVHModel<KDOP<24> >* obj2 = (BVHModel<KDOP<24> >*)o2;
- BVHModel<KDOP<24> >* obj1_tmp = new BVHModel<KDOP<24> >(*obj1);
- SimpleTransform tf1_tmp = tf1;
- BVHModel<KDOP<24> >* obj2_tmp = new BVHModel<KDOP<24> >(*obj2);
- SimpleTransform tf2_tmp = tf2;
- MESHMESH_COMMON_CODE();
-}
+ initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ collide(&node);
+ return BVHContactCollection_OBB_and_RSS_and_kIOS(node.pairs, tf1, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+}
CollisionFunctionMatrix::CollisionFunctionMatrix()
{
@@ -1059,109 +270,109 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
collision_matrix[i][j] = NULL;
}
- collision_matrix[GEOM_BOX][GEOM_BOX] = BoxBoxCollide;
- collision_matrix[GEOM_BOX][GEOM_SPHERE] = BoxSphereCollide;
- collision_matrix[GEOM_BOX][GEOM_CAPSULE] = BoxCapCollide;
- collision_matrix[GEOM_BOX][GEOM_CONE] = BoxConeCollide;
- collision_matrix[GEOM_BOX][GEOM_CYLINDER] = BoxCylinderCollide;
- collision_matrix[GEOM_BOX][GEOM_CONVEX] = BoxConvexCollide;
- collision_matrix[GEOM_BOX][GEOM_PLANE] = BoxPlaneCollide;
-
- collision_matrix[GEOM_SPHERE][GEOM_BOX] = SphereBoxCollide;
- collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = SphereSphereCollide;
- collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = SphereCapCollide;
- collision_matrix[GEOM_SPHERE][GEOM_CONE] = SphereConeCollide;
- collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = SphereCylinderCollide;
- collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = SphereConvexCollide;
- collision_matrix[GEOM_SPHERE][GEOM_PLANE] = SpherePlaneCollide;
-
- collision_matrix[GEOM_CAPSULE][GEOM_BOX] = CapBoxCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = CapSphereCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = CapCapCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_CONE] = CapConeCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = CapCylinderCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = CapConvexCollide;
- collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = CapPlaneCollide;
-
- collision_matrix[GEOM_CONE][GEOM_BOX] = ConeBoxCollide;
- collision_matrix[GEOM_CONE][GEOM_SPHERE] = ConeSphereCollide;
- collision_matrix[GEOM_CONE][GEOM_CAPSULE] = ConeCapCollide;
- collision_matrix[GEOM_CONE][GEOM_CONE] = ConeConeCollide;
- collision_matrix[GEOM_CONE][GEOM_CYLINDER] = ConeCylinderCollide;
- collision_matrix[GEOM_CONE][GEOM_CONVEX] = ConeConvexCollide;
- collision_matrix[GEOM_CONE][GEOM_PLANE] = ConePlaneCollide;
-
- collision_matrix[GEOM_CYLINDER][GEOM_BOX] = CylinderBoxCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = CylinderSphereCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = CylinderCapCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_CONE] = CylinderConeCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = CylinderCylinderCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = CylinderConvexCollide;
- collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = CylinderPlaneCollide;
-
- collision_matrix[GEOM_CONVEX][GEOM_BOX] = ConvexBoxCollide;
- collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = ConvexSphereCollide;
- collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = ConvexCapCollide;
- collision_matrix[GEOM_CONVEX][GEOM_CONE] = ConvexConeCollide;
- collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = ConvexCylinderCollide;
- collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = ConvexConvexCollide;
- collision_matrix[GEOM_CONVEX][GEOM_PLANE] = ConvexPlaneCollide;
-
- collision_matrix[GEOM_PLANE][GEOM_BOX] = PlaneBoxCollide;
- collision_matrix[GEOM_PLANE][GEOM_SPHERE] = PlaneSphereCollide;
- collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = PlaneCapCollide;
- collision_matrix[GEOM_PLANE][GEOM_CONE] = PlaneConeCollide;
- collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = PlaneCylinderCollide;
- collision_matrix[GEOM_PLANE][GEOM_CONVEX] = PlaneConvexCollide;
- collision_matrix[GEOM_PLANE][GEOM_PLANE] = PlanePlaneCollide;
-
- collision_matrix[BV_AABB][GEOM_BOX] = AABBBoxCollide;
- collision_matrix[BV_AABB][GEOM_SPHERE] = AABBSphereCollide;
- collision_matrix[BV_AABB][GEOM_CAPSULE] = AABBCapCollide;
- collision_matrix[BV_AABB][GEOM_CONE] = AABBConeCollide;
- collision_matrix[BV_AABB][GEOM_CYLINDER] = AABBCylinderCollide;
- collision_matrix[BV_AABB][GEOM_CONVEX] = AABBConvexCollide;
- collision_matrix[BV_AABB][GEOM_PLANE] = AABBPlaneCollide;
-
- collision_matrix[BV_OBB][GEOM_BOX] = OBBBoxCollide;
- collision_matrix[BV_OBB][GEOM_SPHERE] = OBBSphereCollide;
- collision_matrix[BV_OBB][GEOM_CAPSULE] = OBBCapCollide;
- collision_matrix[BV_OBB][GEOM_CONE] = OBBConeCollide;
- collision_matrix[BV_OBB][GEOM_CYLINDER] = OBBCylinderCollide;
- collision_matrix[BV_OBB][GEOM_CONVEX] = OBBConvexCollide;
- collision_matrix[BV_OBB][GEOM_PLANE] = OBBPlaneCollide;
-
- collision_matrix[BV_RSS][GEOM_BOX] = RSSBoxCollide;
- collision_matrix[BV_RSS][GEOM_SPHERE] = RSSSphereCollide;
- collision_matrix[BV_RSS][GEOM_CAPSULE] = RSSCapCollide;
- collision_matrix[BV_RSS][GEOM_CONE] = RSSConeCollide;
- collision_matrix[BV_RSS][GEOM_CYLINDER] = RSSCylinderCollide;
- collision_matrix[BV_RSS][GEOM_CONVEX] = RSSConvexCollide;
- collision_matrix[BV_RSS][GEOM_PLANE] = RSSPlaneCollide;
-
- collision_matrix[BV_KDOP16][GEOM_BOX] = KDOP16BoxCollide;
- collision_matrix[BV_KDOP16][GEOM_SPHERE] = KDOP16SphereCollide;
- collision_matrix[BV_KDOP16][GEOM_CAPSULE] = KDOP16CapCollide;
- collision_matrix[BV_KDOP16][GEOM_CONE] = KDOP16ConeCollide;
- collision_matrix[BV_KDOP16][GEOM_CYLINDER] = KDOP16CylinderCollide;
- collision_matrix[BV_KDOP16][GEOM_CONVEX] = KDOP16ConvexCollide;
- collision_matrix[BV_KDOP16][GEOM_PLANE] = KDOP16PlaneCollide;
-
- collision_matrix[BV_KDOP18][GEOM_BOX] = KDOP18BoxCollide;
- collision_matrix[BV_KDOP18][GEOM_SPHERE] = KDOP18SphereCollide;
- collision_matrix[BV_KDOP18][GEOM_CAPSULE] = KDOP18CapCollide;
- collision_matrix[BV_KDOP18][GEOM_CONE] = KDOP18ConeCollide;
- collision_matrix[BV_KDOP18][GEOM_CYLINDER] = KDOP18CylinderCollide;
- collision_matrix[BV_KDOP18][GEOM_CONVEX] = KDOP18ConvexCollide;
- collision_matrix[BV_KDOP18][GEOM_PLANE] = KDOP18PlaneCollide;
-
- collision_matrix[BV_KDOP24][GEOM_BOX] = KDOP24BoxCollide;
- collision_matrix[BV_KDOP24][GEOM_SPHERE] = KDOP24SphereCollide;
- collision_matrix[BV_KDOP24][GEOM_CAPSULE] = KDOP24CapCollide;
- collision_matrix[BV_KDOP24][GEOM_CONE] = KDOP24ConeCollide;
- collision_matrix[BV_KDOP24][GEOM_CYLINDER] = KDOP24CylinderCollide;
- collision_matrix[BV_KDOP24][GEOM_CONVEX] = KDOP24ConvexCollide;
- collision_matrix[BV_KDOP24][GEOM_PLANE] = KDOP24PlaneCollide;
+ collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box, Box>;
+ collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box, Sphere>;
+ collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box, Capsule>;
+ collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box, Cone>;
+ collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder>;
+ collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, Convex>;
+ collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane>;
+
+ collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box>;
+ collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere>;
+ collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere, Capsule>;
+ collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere, Cone>;
+ collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder>;
+ collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere, Convex>;
+ collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane>;
+
+ collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box>;
+ collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule, Capsule>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule, Cone>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule, Convex>;
+ collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule, Plane>;
+
+ collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box>;
+ collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere>;
+ collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone, Capsule>;
+ collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone, Cone>;
+ collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder>;
+ collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone, Convex>;
+ collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane>;
+
+ collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box>;
+ collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder, Capsule>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder, Cone>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder, Convex>;
+ collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder, Plane>;
+
+ collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex, Box>;
+ collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex, Sphere>;
+ collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<Convex, Capsule>;
+ collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<Convex, Cone>;
+ collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex, Cylinder>;
+ collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex, Convex>;
+ collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex, Plane>;
+
+ collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box>;
+ collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere>;
+ collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane, Capsule>;
+ collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane, Cone>;
+ collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder>;
+ collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane, Convex>;
+ collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane>;
+
+ collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box>::collide;
+ collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere>::collide;
+ collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB, Capsule>::collide;
+ collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB, Cone>::collide;
+ collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder>::collide;
+ collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB, Convex>::collide;
+ collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB, Plane>::collide;
+
+ collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box>::collide;
+ collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere>::collide;
+ collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB, Capsule>::collide;
+ collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB, Cone>::collide;
+ collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder>::collide;
+ collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB, Convex>::collide;
+ collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB, Plane>::collide;
+
+ collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box>::collide;
+ collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere>::collide;
+ collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS, Capsule>::collide;
+ collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS, Cone>::collide;
+ collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder>::collide;
+ collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS, Convex>::collide;
+ collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS, Plane>::collide;
+
+ collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box>::collide;
+ collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<16>, Capsule>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<16>, Cone>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<16>, Convex>::collide;
+ collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<16>, Plane>::collide;
+
+ collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box>::collide;
+ collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<18>, Capsule>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<18>, Cone>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<18>, Convex>::collide;
+ collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<18>, Plane>::collide;
+
+ collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box>::collide;
+ collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<24>, Capsule>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<24>, Cone>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, Convex>::collide;
+ collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane>::collide;
/*
collision_matrix[GEOM_BOX][BV_AABB] = BoxAABBCollide;
@@ -1213,12 +424,12 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
collision_matrix[GEOM_PLANE][BV_KDOP24] = PlaneKDOP24Collide;
*/
- collision_matrix[BV_AABB][BV_AABB] = AABBAABBCollide;
- collision_matrix[BV_OBB][BV_OBB] = OBBOBBCollide;
- collision_matrix[BV_RSS][BV_RSS] = RSSRSSCollide;
- collision_matrix[BV_KDOP16][BV_KDOP16] = KDOP16KDOP16Collide;
- collision_matrix[BV_KDOP18][BV_KDOP18] = KDOP18KDOP18Collide;
- collision_matrix[BV_KDOP24][BV_KDOP24] = KDOP24KDOP24Collide;
+ collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB>;
+ collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB>;
+ collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS>;
+ collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<16> >;
+ collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<18> >;
+ collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24> >;
}
}
diff --git a/trunk/fcl/src/collision_node.cpp b/trunk/fcl/src/collision_node.cpp
index 70d1775fb87a241d2d8cb2c8d351302be95cc1b9..de3095a0330ccb4c48af2e063658d02ffc579eb9 100644
--- a/trunk/fcl/src/collision_node.cpp
+++ b/trunk/fcl/src/collision_node.cpp
@@ -141,5 +141,37 @@ void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list, int
node->p2 = node->R.transposeTimes(u);
}
+void distance(MeshDistanceTraversalNodekIOS* node, BVHFrontList* front_list, int qsize)
+{
+ Triangle last_tri1 = node->tri_indices1[node->last_tri_id1];
+ Triangle last_tri2 = node->tri_indices2[node->last_tri_id2];
+
+ Vec3f last_tri1_points[3];
+ Vec3f last_tri2_points[3];
+
+ last_tri1_points[0] = node->vertices1[last_tri1[0]];
+ last_tri1_points[1] = node->vertices1[last_tri1[1]];
+ last_tri1_points[2] = node->vertices1[last_tri1[2]];
+
+ last_tri2_points[0] = node->vertices2[last_tri2[0]];
+ last_tri2_points[1] = node->vertices2[last_tri2[1]];
+ last_tri2_points[2] = node->vertices2[last_tri2[2]];
+
+ Vec3f last_tri_P, last_tri_Q;
+
+ node->min_distance = TriangleDistance::triDistance(last_tri1_points[0], last_tri1_points[1], last_tri1_points[2],
+ last_tri2_points[0], last_tri2_points[1], last_tri2_points[2],
+ node->R, node->T, last_tri_P, last_tri_Q);
+ node->p1 = last_tri_P;
+ node->p2 = node->R.transposeTimes(last_tri_Q - node->T);
+
+ if(qsize <= 2)
+ distanceRecurse(node, 0, 0, front_list);
+ else
+ distanceQueueRecurse(node, 0, 0, front_list, qsize);
+
+ Vec3f u = node->p2 - node->T;
+ node->p2 = node->R.transposeTimes(u);
+}
}
diff --git a/trunk/fcl/src/kIOS.cpp b/trunk/fcl/src/kIOS.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..210255743bba1631b2e88ba6dd6c6ef00522f6f6
--- /dev/null
+++ b/trunk/fcl/src/kIOS.cpp
@@ -0,0 +1,191 @@
+/*
+ * 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/kIOS.h"
+#include "fcl/BVH_utility.h"
+#include "fcl/transform.h"
+
+#include <iostream>
+#include <limits>
+
+namespace fcl
+{
+
+bool kIOS::overlap(const kIOS& other) const
+{
+ for(unsigned int i = 0; i < num_spheres; ++i)
+ {
+ for(unsigned int j = 0; j < other.num_spheres; ++j)
+ {
+ BVH_REAL o_dist = (spheres[i].o - other.spheres[j].o).sqrLength();
+ BVH_REAL sum_r = spheres[i].r + other.spheres[j].r;
+ if(o_dist > sum_r * sum_r)
+ return false;
+ }
+ }
+
+ return true;
+}
+
+
+bool kIOS::contain(const Vec3f& p) const
+{
+ for(unsigned int i = 0; i < num_spheres; ++i)
+ {
+ BVH_REAL r = spheres[i].r;
+ if((spheres[i].o - p).sqrLength() > r * r)
+ return false;
+ }
+
+ return true;
+}
+
+kIOS& kIOS::operator += (const Vec3f& p)
+{
+ for(unsigned int i = 0; i < num_spheres; ++i)
+ {
+ BVH_REAL r = spheres[i].r;
+ BVH_REAL new_r_sqr = (p - spheres[i].o).sqrLength();
+ if(new_r_sqr > r * r)
+ {
+ spheres[i].r = sqrt(new_r_sqr);
+ }
+ }
+}
+
+kIOS kIOS::operator + (const kIOS& other) const
+{
+ kIOS result;
+ unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres);
+ for(unsigned int i = 0; i < new_num_spheres; ++i)
+ {
+ result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]);
+ }
+
+ result.num_spheres = new_num_spheres;
+ return result;
+}
+
+BVH_REAL kIOS::width() const
+{
+ BVH_REAL min_r = std::numeric_limits<BVH_REAL>::max();
+ for(unsigned int i = 0; i < num_spheres; ++i)
+ {
+ if(spheres[i].r < min_r) min_r = spheres[i].r;
+ }
+ return min_r;
+}
+
+BVH_REAL kIOS::height() const
+{
+ return width();
+}
+
+BVH_REAL kIOS::depth() const
+{
+ return width();
+}
+
+BVH_REAL kIOS::volume() const
+{
+ BVH_REAL r = kIOS::width();
+ return 4.0 / 3.0 * 3.1415926 * r * r * r;
+}
+
+BVH_REAL kIOS::size() const
+{
+ return volume();
+}
+
+BVH_REAL kIOS::distance(const kIOS& other, Vec3f* P, Vec3f* Q) const
+{
+ BVH_REAL d_max = 0;
+ unsigned int id_a = -1, id_b = -1;
+ for(unsigned int i = 0; i < num_spheres; ++i)
+ {
+ for(unsigned int j = 0; j < other.num_spheres; ++j)
+ {
+ BVH_REAL d = (spheres[i].o - other.spheres[j].o).length() - (spheres[i].r + other.spheres[j].r);
+ if(d_max < d)
+ {
+ d_max = d;
+ if(P && Q)
+ {
+ id_a = i; id_b = j;
+ }
+ }
+ }
+ }
+
+ if(P && Q)
+ {
+ if(id_a != -1 && id_b != -1)
+ {
+ Vec3f v = spheres[id_a].o - spheres[id_b].o;
+ BVH_REAL len_v = v.length();
+ *P = spheres[id_a].o - v * (spheres[id_a].r / len_v);
+ *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v);
+ }
+ }
+
+ return d_max;
+}
+
+
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2)
+{
+ kIOS b2_temp = b2;
+ for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
+ {
+ b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
+ }
+
+ return b1.overlap(b2_temp);
+}
+
+BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
+{
+ kIOS b2_temp = b2;
+ for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
+ {
+ b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
+ }
+
+ return b1.distance(b2_temp, P, Q);
+}
+
+
+}
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index fa2fb1f668b69c3c13f29a96a99778defcdec381..e2142c2ae5b1809b0252112760d2ce866ec80b87 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -98,6 +98,35 @@ bool initialize(MeshCollisionTraversalNodeRSS& node,
return true;
}
+
+bool initialize(MeshCollisionTraversalNodekIOS& node,
+ const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
+ 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;
+}
+
#if USE_SVMLIGHT
bool initialize(PointCloudCollisionTraversalNodeOBB& node,
@@ -290,4 +319,28 @@ bool initialize(MeshDistanceTraversalNodeRSS& node,
}
+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;
+}
+
+
}
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 32bcccdfdb9a348d009c2042afbe72790a61d364..e46da0a478d4b6550b3e683527fb749783a1b37c 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -220,6 +220,70 @@ void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
}
}
+
+
+
+MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS() : MeshCollisionTraversalNode<kIOS>()
+{
+ R.setIdentity();
+ // default T is 0
+}
+
+bool MeshCollisionTraversalNodekIOS::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 MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
+{
+ if(enable_statistics) num_leaf_tests++;
+
+ const BVNode<kIOS>& node1 = model1->getBV(b1);
+ const BVNode<kIOS>& node2 = model2->getBV(b2);
+
+ int primitive_id1 = node1.primitiveId();
+ int primitive_id2 = node2.primitiveId();
+
+ const Triangle& tri_id1 = tri_indices1[primitive_id1];
+ const Triangle& tri_id2 = tri_indices2[primitive_id2];
+
+ const Vec3f& p1 = vertices1[tri_id1[0]];
+ const Vec3f& p2 = vertices1[tri_id1[1]];
+ const Vec3f& p3 = vertices1[tri_id1[2]];
+ const Vec3f& q1 = vertices2[tri_id2[0]];
+ const Vec3f& q2 = vertices2[tri_id2[1]];
+ const Vec3f& q3 = vertices2[tri_id2[2]];
+
+ BVH_REAL penetration;
+ Vec3f normal;
+ int n_contacts;
+ Vec3f contacts[2];
+
+
+ if(!enable_contact) // only interested in collision or not
+ {
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3, R, T))
+ pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2));
+ }
+ else // need compute the contact information
+ {
+ if(Intersect::intersect_Triangle(p1, p2, p3, q1, q2, q3,
+ R, T,
+ contacts,
+ (unsigned int*)&n_contacts,
+ &penetration,
+ &normal))
+ {
+ for(int i = 0; i < n_contacts; ++i)
+ {
+ if((!exhaustive) && (num_max_contacts <= (int)pairs.size())) break;
+ pairs.push_back(BVHCollisionPair(primitive_id1, primitive_id2, contacts[i], normal, penetration));
+ }
+ }
+ }
+}
+
#if USE_SVMLIGHT
PointCloudCollisionTraversalNodeOBB::PointCloudCollisionTraversalNodeOBB() : PointCloudCollisionTraversalNode<OBB>()
@@ -421,6 +485,57 @@ void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const
}
}
+MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() : MeshDistanceTraversalNode<kIOS>()
+{
+ R.setIdentity();
+ // default T is 0
+}
+
+BVH_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const
+{
+ if(enable_statistics) num_bv_tests++;
+ return distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv);
+}
+
+void MeshDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const
+{
+ if(enable_statistics) num_leaf_tests++;
+
+ const BVNode<kIOS>& node1 = model1->getBV(b1);
+ const BVNode<kIOS>& node2 = model2->getBV(b2);
+
+ int primitive_id1 = node1.primitiveId();
+ int primitive_id2 = node2.primitiveId();
+
+ const Triangle& tri_id1 = tri_indices1[primitive_id1];
+ const Triangle& tri_id2 = tri_indices2[primitive_id2];
+
+ const Vec3f& t11 = vertices1[tri_id1[0]];
+ const Vec3f& t12 = vertices1[tri_id1[1]];
+ const Vec3f& t13 = vertices1[tri_id1[2]];
+
+ const Vec3f& t21 = vertices2[tri_id2[0]];
+ const Vec3f& t22 = vertices2[tri_id2[1]];
+ const Vec3f& t23 = vertices2[tri_id2[2]];
+
+ // nearest point pair
+ Vec3f P1, P2;
+
+ BVH_REAL d = TriangleDistance::triDistance(t11, t12, t13, t21, t22, t23,
+ R, T,
+ P1, P2);
+
+ if(d < min_distance)
+ {
+ min_distance = d;
+
+ p1 = P1;
+ p2 = P2;
+
+ last_tri_id1 = primitive_id1;
+ last_tri_id2 = primitive_id2;
+ }
+}
/** for OBB and RSS, there is local coordinate of BV, so normal need to be transformed */