From 1711279068bdd4d3732170919d2952a204bb7ef5 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Wed, 28 Sep 2011 06:20:10 +0000
Subject: [PATCH] This line, and those below, will be ignored--
M fcl/test/test_core_utility.h
M fcl/test/test_core_distance.cpp
M fcl/test/test_core_continuous_collision.cpp
M fcl/include/fcl/traversal_node_bvhs.h
M fcl/include/fcl/motion_base.h
M fcl/include/fcl/simple_setup.h
M fcl/include/fcl/motion.h
M fcl/include/fcl/transform.h
M fcl/manifest.xml
M fcl/src/motion.cpp
M fcl/src/traversal_node_bvhs.cpp
M fcl/CMakeLists.txt
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@35 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 10 ++-
trunk/fcl/include/fcl/motion.h | 57 ++++++++++------
trunk/fcl/include/fcl/motion_base.h | 2 +-
trunk/fcl/include/fcl/simple_setup.h | 6 --
trunk/fcl/include/fcl/transform.h | 2 +-
trunk/fcl/include/fcl/traversal_node_bvhs.h | 43 ++++++------
trunk/fcl/manifest.xml | 1 +
trunk/fcl/src/motion.cpp | 8 +--
trunk/fcl/src/traversal_node_bvhs.cpp | 48 +++++++-------
.../test/test_core_continuous_collision.cpp | 61 ++++++++++++++++-
trunk/fcl/test/test_core_distance.cpp | 65 +++++++++++++++++++
trunk/fcl/test/test_core_utility.h | 1 +
12 files changed, 221 insertions(+), 83 deletions(-)
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 5fae2c40..67bff808 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -32,7 +32,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
add_definitions(-DUSE_PQP=0)
add_definitions(-DUSE_SVMLIGHT=0)
-rosbuild_add_library(${PROJECT_NAME} 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)
+rosbuild_add_library(${PROJECT_NAME} 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)
rosbuild_add_gtest(test_core_collision test/test_core_collision.cpp)
target_link_libraries(test_core_collision fcl)
@@ -56,4 +56,10 @@ rosbuild_add_gtest(test_core_front_list test/test_core_front_list.cpp)
target_link_libraries(test_core_front_list fcl)
rosbuild_add_gtest(test_core_continuous_collision test/test_core_continuous_collision.cpp)
-target_link_libraries(test_core_continuous_collision fcl)
\ No newline at end of file
+target_link_libraries(test_core_continuous_collision fcl)
+
+rosbuild_add_executable(test_core_deformable_object test/test_core_deformable_object.cpp)
+target_link_libraries(test_core_deformable_object fcl)
+
+rosbuild_add_executable(test_core_conservative_advancement test/test_core_conservative_advancement.cpp)
+target_link_libraries(test_core_conservative_advancement fcl)
\ No newline at end of file
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index fb0e2790..c457ab5a 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -48,6 +48,10 @@ namespace fcl
/** \brief Linear interpolation motion
* Each Motion is assumed to have constant linear velocity and angular velocity
+ * The motion is R(t)(p - p_ref) + p_ref + T(t)
+ * Therefore, R(0) = R0, R(1) = R1
+ * T(0) = T0 + R0 p_ref - p_ref
+ * T(1) = T1 + R1 p_ref - p_ref
*/
template<typename BV>
class InterpMotion : public MotionBase<BV>
@@ -60,6 +64,8 @@ public:
angular_axis = Vec3f(1, 0, 0);
angular_vel = 0;
+ /** Default reference point is local zero point */
+
/** Default linear velocity is zero */
}
@@ -73,6 +79,8 @@ public:
/** Current time is zero, so the transformation is t1 */
t = t1;
+ /** Default reference point is local zero point */
+
/** Compute the velocities for the motion */
computeVelocity();
}
@@ -87,6 +95,8 @@ public:
t2 = SimpleTransform(R2, T2 - matMulVec(R2, O));
t = t1;
+ reference_p = O;
+
/** Compute the velocities for the motion */
computeVelocity();
}
@@ -99,33 +109,29 @@ public:
{
if(dt > 1) dt = 1;
- t.T = t1.T + linear_vel * dt;
-
- t.q = absoluteRotation(dt);
- t.q.toRotation(t.R);
+ t.setQuatRotation(absoluteRotation(dt));
+ t.setTranslation(linear_vel * dt + t1.transform(reference_p) - t.getQuatRotation().transform(reference_p));
return true;
}
- /** \brief Compute the motion bound for a bounding volume, given the closest direction n between two query objects
- * according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
- * and ci are the endpoints of the generator primitives of RSS.
- * Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
+ /** \brief Compute the motion bound for a bounding volume along a given direction n
+ * For general BV, not implemented so return trivial 0
*/
BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
- /** \brief Compute the motion bound for a triangle, given the closest direction n between two query objects
- * according to mu < |v * | + ||w x n||(max||ci*||) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
+ /** \brief Compute the motion bound for a triangle along a given direction n
+ * according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
* and ci are the triangle vertex coordinates.
* Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
*/
BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
{
- BVH_REAL c_proj_max = (a.cross(angular_axis)).sqrLength();
+ BVH_REAL c_proj_max = ((a - reference_p).cross(angular_axis)).sqrLength();
BVH_REAL tmp;
- tmp = (b.cross(angular_axis)).sqrLength();
+ tmp = ((b - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = (c.cross(angular_axis)).sqrLength();
+ tmp = ((c - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = sqrt(c_proj_max);
@@ -138,33 +144,33 @@ public:
}
/** \brief Get the rotation and translation in current step */
- void getCurrentTransformation(Vec3f R[3], Vec3f& T) const
+ void getCurrentTransform(Vec3f R[3], Vec3f& T) const
{
for(int i = 0; i < 3; ++i)
{
- R[i] = t.R[i];
+ R[i] = t.getRotation()[i];
}
- T = t.T;
+ T = t.getTranslation();
}
void getCurrentRotation(Vec3f R[3]) const
{
for(int i = 0; i < 3; ++i)
- R[i] = t.R[i];
+ R[i] = t.getRotation()[i];
}
void getCurrentTranslation(Vec3f& T) const
{
- T = t.T;
+ T = t.getTranslation();
}
protected:
void computeVelocity()
{
- linear_vel = t2.T - t1.T;
- SimpleQuaternion deltaq = t2.q * t1.q.inverse();
+ linear_vel = t2.transform(reference_p) - t1.transform(reference_p);
+ SimpleQuaternion deltaq = t2.getQuatRotation() * t1.getQuatRotation().inverse();
deltaq.toAxisAngle(angular_axis, angular_vel);
}
@@ -179,7 +185,7 @@ protected:
SimpleQuaternion absoluteRotation(BVH_REAL t) const
{
SimpleQuaternion delta_t = deltaRotation(t);
- return delta_t * t1.q;
+ return delta_t * t1.getQuatRotation();
}
/** \brief The transformation at time 0 */
@@ -199,8 +205,17 @@ protected:
/** \brief Angular velocity axis */
Vec3f angular_axis;
+
+ /** \brief Reference point for the motion (in the object's local frame) */
+ Vec3f reference_p;
};
+
+/** \brief Compute the motion bound for a bounding volume along a given direction n
+ * according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
+ * and ci are the endpoints of the generator primitives of RSS.
+ * Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
+ */
template<>
BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
diff --git a/trunk/fcl/include/fcl/motion_base.h b/trunk/fcl/include/fcl/motion_base.h
index 0771dbd5..005a3d74 100644
--- a/trunk/fcl/include/fcl/motion_base.h
+++ b/trunk/fcl/include/fcl/motion_base.h
@@ -59,7 +59,7 @@ public:
virtual BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const = 0;
/** \brief Get the rotation and translation in current step */
- virtual void getCurrentTransformation(Vec3f R[3], Vec3f& T) const = 0;
+ virtual void getCurrentTransform(Vec3f R[3], Vec3f& T) const = 0;
virtual void getCurrentRotation(Vec3f R[3]) const = 0;
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index f08bf14b..6ac27527 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -616,10 +616,6 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
node.w = w;
- //HOW?
- //node.motion1 = new InterpMotion<BV>
- //node.motion2 = new InterpMotion<BV>
-
return true;
}
@@ -644,8 +640,6 @@ inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const
relativeTransform(R1, T1, R2, T2, node.R, node.T);
-
-
return true;
}
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/transform.h
index c4f307fc..2db81c9f 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/transform.h
@@ -207,7 +207,7 @@ public:
bool isIdentity() const
{
- return (R[0][0] == 1) && (R[0][1] == 0) && (R[0][2] == 0) && (R[1][0] == 0) && (R[1][1] == 1) && (R[1][2] == 1) && (R[2][0] == 0) && (R[2][1] == 0) && (R[2][2] == 1)
+ return (R[0][0] == 1) && (R[0][1] == 0) && (R[0][2] == 0) && (R[1][0] == 0) && (R[1][1] == 1) && (R[1][2] == 0) && (R[2][0] == 0) && (R[2][1] == 0) && (R[2][2] == 1)
&& (T[0] == 0) && (T[1] == 0) && (T[2] == 0);
}
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index 9ab9265c..fa1f4d2b 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -1038,6 +1038,19 @@ public:
Vec3f T;
};
+
+
+struct ConservativeAdvancementStackData
+{
+ ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, BVH_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
+
+ Vec3f P1;
+ Vec3f P2;
+ int c1;
+ int c2;
+ BVH_REAL d;
+};
+
// when using this default version, must refit the BVH in current configuration (R_t, T_t) into default configuration
template<typename BV>
class MeshConservativeAdvancementTraversalNode : public MeshDistanceTraversalNode<BV>
@@ -1061,7 +1074,7 @@ public:
Vec3f P1, P2;
BVH_REAL d = this->model1->getBV(b1).distance(this->model2->getBV(b2), &P1, &P2);
- stack.push_back(StackData(P1, P2, b1, b2, d));
+ stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
@@ -1105,11 +1118,9 @@ public:
}
- /** n is the local frame of object 1 */
+ // n is the local frame of object 1
Vec3f n = P2 - P1;
- /** turn n into the global frame
- * nothing is done here because in this case we assume the body is in original configuration (I, 0)
- */
+ // here n is already in global frame as we assume the body is in original configuration (I, 0) for general BVH
BVH_REAL bound1 = motion1->computeMotionBound(p1, p2, p3, n);
BVH_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);
@@ -1124,14 +1135,14 @@ public:
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
- const StackData& data = stack.back();
+ const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
Vec3f n;
int c1, c2;
if(d > c)
{
- const StackData& data2 = stack[stack.size() - 2];
+ const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
n = data2.P2 - data2.P1;
c1 = data2.c1;
@@ -1147,9 +1158,6 @@ public:
if(c != d) std::cout << "there is some problem here" << std::endl;
-
- // (this->tree1 + c1)->bv.axis[0] * n[0] + (this->tree1 + c1)->bv.axis[1] * n[1] + (this->tree1 + c1)->bv.axis[2] * n[2];
-
BVH_REAL bound1 = motion1->computeMotionBound((this->tree1 + c1)->bv, n);
BVH_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);
@@ -1163,7 +1171,7 @@ public:
}
else
{
- const StackData& data = stack.back();
+ const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
if(d > c)
@@ -1190,18 +1198,7 @@ public:
MotionBase<BV>* motion1;
MotionBase<BV>* motion2;
- struct StackData
- {
- StackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, BVH_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
-
- Vec3f P1;
- Vec3f P2;
- int c1;
- int c2;
- BVH_REAL d;
- };
-
- mutable std::vector<StackData> stack;
+ mutable std::vector<ConservativeAdvancementStackData> stack;
};
diff --git a/trunk/fcl/manifest.xml b/trunk/fcl/manifest.xml
index 370fb5d3..8d4c2ba2 100644
--- a/trunk/fcl/manifest.xml
+++ b/trunk/fcl/manifest.xml
@@ -10,6 +10,7 @@
<url>http://ros.org/wiki/fcl</url>
<depend package="libccd"/>
<depend package="ann"/>
+ <depend package="assimp"/>
<!-- <depend package="PQP"/> -->
<!-- <depend package="svm_light"/> -->
<export>
diff --git a/trunk/fcl/src/motion.cpp b/trunk/fcl/src/motion.cpp
index 3f2c9568..cc20b258 100644
--- a/trunk/fcl/src/motion.cpp
+++ b/trunk/fcl/src/motion.cpp
@@ -43,13 +43,13 @@ namespace fcl
template<>
BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
{
- BVH_REAL c_proj_max = (bv.Tr.cross(angular_axis)).sqrLength();
+ BVH_REAL c_proj_max = ((bv.Tr - reference_p).cross(angular_axis)).sqrLength();
BVH_REAL tmp;
- tmp = ((bv.Tr + bv.axis[0] * bv.l[0]).cross(angular_axis)).sqrLength();
+ tmp = ((bv.Tr + bv.axis[0] * bv.l[0] - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((bv.Tr + bv.axis[1] * bv.l[1]).cross(angular_axis)).sqrLength();
+ tmp = ((bv.Tr + bv.axis[1] * bv.l[1] - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1]).cross(angular_axis)).sqrLength();
+ tmp = ((bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = sqrt(c_proj_max);
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 8fb13f5f..0abc9322 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -393,14 +393,14 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
- const StackData& data = stack.back();
+ const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
Vec3f n;
int c1, c2;
if(d > c)
{
- const StackData& data2 = stack[stack.size() - 2];
+ const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
n = data2.P2 - data2.P1;
c1 = data2.c1;
@@ -431,7 +431,7 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
}
else
{
- const StackData& data = stack.back();
+ const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
if(d > c)
@@ -448,14 +448,14 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
{
if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
{
- const StackData& data = stack.back();
+ const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
Vec3f n;
int c1, c2;
if(d > c)
{
- const StackData& data2 = stack[stack.size() - 2];
+ const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
n = data2.P2 - data2.P1;
c1 = data2.c1;
@@ -486,7 +486,7 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
}
else
{
- const StackData& data = stack.back();
+ const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
if(d > c)
@@ -514,7 +514,7 @@ BVH_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2)
Vec3f P1, P2;
BVH_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
- stack.push_back(StackData(P1, P2, b1, b2, d));
+ stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
return d;
}
@@ -560,17 +560,20 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
}
- /** n is the local frame of object 1 */
+ /** n is the local frame of object 1, pointing from object 1 to object2 */
Vec3f n = P2 - P1;
- /** turn n into the global frame */
- Vec3f R[3];
- motion1->getCurrentRotation(R);
- Vec3f n_transformed = matMulVec(R, n);
+ /** turn n into the global frame, pointing from object 1 to object 2 */
+ Vec3f R0[3];
+ motion1->getCurrentRotation(R0);
+ Vec3f n_transformed = matMulVec(R0, n);
n_transformed.normalize();
BVH_REAL bound1 = motion1->computeMotionBound(t11, t12, t13, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, n_transformed);
+ BVH_REAL bound2 = motion2->computeMotionBound(t21, t22, t23, -n_transformed);
- BVH_REAL cur_delta_t = d / (bound1 + bound2);
+ BVH_REAL bound = bound1 + bound2;
+ if(bound < d) bound = d;
+
+ BVH_REAL cur_delta_t = d / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
@@ -580,14 +583,14 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
{
if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
{
- const StackData& data = stack.back();
+ const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
Vec3f n;
int c1, c2;
if(d > c)
{
- const StackData& data2 = stack[stack.size() - 2];
+ const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
d = data2.d;
n = data2.P2 - data2.P1;
c1 = data2.c1;
@@ -603,16 +606,15 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
if(c != d) std::cout << "there is some problem here" << std::endl;
- /** n is in local frame of RSS c1 */
- /** turn n into the global frame */
+ // n is in local frame of RSS c1, so we need to turn n into the global frame
Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[2] + model1->getBV(c1).bv.axis[2] * n[2];
- Vec3f R[3];
- motion1->getCurrentRotation(R);
- n_transformed = matMulVec(R, n_transformed);
+ Vec3f R0[3];
+ motion1->getCurrentRotation(R0);
+ n_transformed = matMulVec(R0, n_transformed);
n_transformed.normalize();
BVH_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
- BVH_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, n_transformed);
+ BVH_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
BVH_REAL cur_delta_t = c / (bound1 + bound2);
if(cur_delta_t < delta_t)
@@ -624,7 +626,7 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
}
else
{
- const StackData& data = stack.back();
+ const ConservativeAdvancementStackData& data = stack.back();
BVH_REAL d = data.d;
if(d > c)
diff --git a/trunk/fcl/test/test_core_continuous_collision.cpp b/trunk/fcl/test/test_core_continuous_collision.cpp
index 8fd1bd53..e8f536d7 100644
--- a/trunk/fcl/test/test_core_continuous_collision.cpp
+++ b/trunk/fcl/test/test_core_continuous_collision.cpp
@@ -39,6 +39,7 @@
#include "fcl/simple_setup.h"
#include "test_core_utility.h"
#include <gtest/gtest.h>
+#include <boost/timer.hpp>
using namespace fcl;
@@ -62,8 +63,13 @@ bool exhaustive = true;
bool enable_contact = true;
unsigned int n_dcd_samples = 10;
-TEST(ccd_test, mesh_mesh)
+
+
+TEST(ccd_test, mesh_mesh_bottomup)
{
+ double t_ccd = 0;
+ double t_dcd = 0;
+
std::vector<Vec3f> p1, p2;
std::vector<Triangle> t1, t2;
loadOBJFile("test/env.obj", p1, t1);
@@ -73,7 +79,7 @@ TEST(ccd_test, mesh_mesh)
std::vector<Transform> transforms2; // t1
BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
BVH_REAL delta_trans[] = {10, 10, 10};
- int n = 1000;
+ int n = 100;
bool verbose = false;
generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
@@ -82,8 +88,13 @@ TEST(ccd_test, mesh_mesh)
for(unsigned int i = 0; i < transforms.size(); ++i)
{
+ boost::timer timer1;
res = discrete_continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, verbose);
+ t_dcd += timer1.elapsed();
+
+ boost::timer timer2;
res2 = continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, true, verbose);
+ t_ccd += timer2.elapsed();
if(res)
ASSERT_TRUE(res == res2);
else
@@ -93,8 +104,54 @@ TEST(ccd_test, mesh_mesh)
}
}
+ std::cout << "dcd timing: " << t_dcd << " sec" << std::endl;
+ std::cout << "ccd timing: " << t_ccd << " sec" << std::endl;
}
+
+TEST(ccd_test, mesh_mesh_topdown)
+{
+ double t_ccd = 0;
+ double t_dcd = 0;
+
+ std::vector<Vec3f> p1, p2;
+ std::vector<Triangle> t1, t2;
+ loadOBJFile("test/env.obj", p1, t1);
+ loadOBJFile("test/rob.obj", p2, t2);
+
+ std::vector<Transform> transforms; // t0
+ std::vector<Transform> transforms2; // t1
+ BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
+ BVH_REAL delta_trans[] = {10, 10, 10};
+ int n = 100;
+ bool verbose = false;
+
+ generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
+
+ bool res, res2;
+
+ for(unsigned int i = 0; i < transforms.size(); ++i)
+ {
+ boost::timer timer1;
+ res = discrete_continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, verbose);
+ t_dcd += timer1.elapsed();
+
+ boost::timer timer2;
+ res2 = continuous_collide_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, verbose);
+ t_ccd += timer2.elapsed();
+ if(res)
+ ASSERT_TRUE(res == res2);
+ else
+ {
+ if(res2)
+ std::cout << "CCD detects collision missed in DCD" << std::endl;
+ }
+ }
+ std::cout << "dcd timing: " << t_dcd << " sec" << std::endl;
+ std::cout << "ccd timing: " << t_ccd << " sec" << std::endl;
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
diff --git a/trunk/fcl/test/test_core_distance.cpp b/trunk/fcl/test/test_core_distance.cpp
index a63898fb..add5b593 100644
--- a/trunk/fcl/test/test_core_distance.cpp
+++ b/trunk/fcl/test/test_core_distance.cpp
@@ -40,6 +40,7 @@
#include "fcl/simple_setup.h"
#include "test_core_utility.h"
#include <gtest/gtest.h>
+#include <boost/timer.hpp>
#if USE_PQP
@@ -64,6 +65,11 @@ void distance_Test2(const Transform& tf,
DistanceRes& distance_result,
bool verbose = true);
+bool collide_Test_OBB(const Transform& tf,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose);
+
+
bool verbose = false;
BVH_REAL DELTA = 0.001;
@@ -92,9 +98,16 @@ TEST(collision_core, mesh_distance)
generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
+ double dis_time = 0;
+ double col_time = 0;
+
DistanceRes res, res_now;
for(unsigned int i = 0; i < transforms.size(); ++i)
{
+ boost::timer timer_col;
+ collide_Test_OBB(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, verbose);
+ col_time += timer_col.elapsed();
+
#if USE_PQP
distance_PQP(transforms[i], p1, t1, p2, t2, res, verbose);
@@ -104,6 +117,7 @@ TEST(collision_core, mesh_distance)
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
#endif
+ boost::timer timer_dist;
#if USE_PQP
distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res_now, verbose);
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
@@ -111,6 +125,7 @@ TEST(collision_core, mesh_distance)
#else
distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN, 2, res, verbose);
#endif
+ dis_time += timer_dist.elapsed();
distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER, 2, res_now, verbose);
@@ -167,11 +182,16 @@ TEST(collision_core, mesh_distance)
ASSERT_TRUE(fabs(res.distance - res_now.distance) < DELTA);
ASSERT_TRUE(fabs(res.distance) < DELTA || (res.distance > 0 && nearlyEqual(res.p1, res_now.p1) && nearlyEqual(res.p2, res_now.p2)));
}
+
+ std::cout << "distance timing: " << dis_time << " sec" << std::endl;
+ std::cout << "collision timing: " << col_time << " sec" << std::endl;
+
}
int main(int argc, char **argv)
{
+ srand(time(NULL));
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
@@ -287,3 +307,48 @@ void distance_Test2(const Transform& tf,
std::cout << node.num_bv_tests << " " << node.num_leaf_tests << std::endl;
}
}
+
+
+bool collide_Test_OBB(const Transform& tf,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose)
+{
+ BVHModel<OBB> m1;
+ BVHModel<OBB> m2;
+ m1.bv_splitter.reset(new BVSplitter<OBB>(split_method));
+ m2.bv_splitter.reset(new BVSplitter<OBB>(split_method));
+
+ m1.beginModel();
+ m1.addSubModel(vertices1, triangles1);
+ m1.endModel();
+
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+
+ Vec3f R2[3];
+ R2[0] = Vec3f(1, 0, 0);
+ R2[1] = Vec3f(0, 1, 0);
+ R2[2] = Vec3f(0, 0, 1);
+ Vec3f T2;
+
+ m1.setTransform(tf.R, tf.T);
+ m2.setTransform(R2, T2);
+
+ MeshCollisionTraversalNodeOBB node;
+ if(!initialize(node, (const BVHModel<OBB>&)m1, (const BVHModel<OBB>&)m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.enable_statistics = verbose;
+ node.num_max_contacts = 1;
+ node.exhaustive = false;
+ node.enable_contact = false;
+
+ collide(&node);
+
+ if(node.pairs.size() > 0)
+ return true;
+ else
+ return false;
+}
+
diff --git a/trunk/fcl/test/test_core_utility.h b/trunk/fcl/test/test_core_utility.h
index 4a098af6..3fb04a29 100644
--- a/trunk/fcl/test/test_core_utility.h
+++ b/trunk/fcl/test/test_core_utility.h
@@ -43,6 +43,7 @@
#include <cstdio>
#include <vector>
#include <iostream>
+#include <string.h>
using namespace fcl;
#if USE_PQP
--
GitLab