diff --git a/trunk/fcl/include/fcl/conservative_advancement.h b/trunk/fcl/include/fcl/conservative_advancement.h
index 62ccbd1cdb6150abc1ebb6702a19f3356edb846e..fdaa8e656aa884e6523c59b27b16358ded18de5f 100644
--- a/trunk/fcl/include/fcl/conservative_advancement.h
+++ b/trunk/fcl/include/fcl/conservative_advancement.h
@@ -50,9 +50,11 @@ namespace fcl
int conservativeAdvancement(const CollisionObject* o1,
const Vec3f R1_1[3], const Vec3f& T1_1,
const Vec3f R1_2[3], const Vec3f& T1_2,
+ const Vec3f& O1,
const CollisionObject* o2,
const Vec3f R2_1[3], const Vec3f& T2_1,
const Vec3f R2_2[3], const Vec3f& T2_2,
+ const Vec3f& O2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts,
BVH_REAL& toc);
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index c457ab5a1c3a3020abe3e726f58c29ccedd6ece7..f42defe895a8d0e94bb4e2c1372a44b5c2f8a587 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -172,6 +172,11 @@ protected:
linear_vel = t2.transform(reference_p) - t1.transform(reference_p);
SimpleQuaternion deltaq = t2.getQuatRotation() * t1.getQuatRotation().inverse();
deltaq.toAxisAngle(angular_axis, angular_vel);
+ if(angular_vel < 0)
+ {
+ angular_vel = -angular_vel;
+ angular_axis = -angular_axis;
+ }
}
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index fa1f4d2bee01c1252aafb61e35f91a510f5e78e4..72088fdb36c2dfbf81ac1a93acd468669118a091 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -1124,7 +1124,10 @@ public:
BVH_REAL bound1 = motion1->computeMotionBound(p1, p2, p3, n);
BVH_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);
- 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;
@@ -1161,7 +1164,10 @@ public:
BVH_REAL bound1 = motion1->computeMotionBound((this->tree1 + c1)->bv, n);
BVH_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);
- BVH_REAL cur_delta_t = c / (bound1 + bound2);
+ BVH_REAL bound = bound1 + bound2;
+ if(bound < c) bound = c;
+
+ BVH_REAL cur_delta_t = c / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp
index f8a1dae8a5b2cef6a86e3cdbe27479ab458220ae..54718c5a12974b036ebe788017386d4e1eb5e312 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/conservative_advancement.cpp
@@ -49,9 +49,11 @@ namespace fcl
int conservativeAdvancement(const CollisionObject* o1,
const Vec3f R1_1[3], const Vec3f& T1_1,
const Vec3f R1_2[3], const Vec3f& T1_2,
+ const Vec3f& O1,
const CollisionObject* o2,
const Vec3f R2_1[3], const Vec3f& T2_1,
const Vec3f R2_2[3], const Vec3f& T2_2,
+ const Vec3f& O2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts,
BVH_REAL& toc)
@@ -74,26 +76,49 @@ int conservativeAdvancement(const CollisionObject* o1,
if(node_type1 != BV_RSS || node_type2 != BV_RSS)
return 0;
- MeshConservativeAdvancementTraversalNodeRSS node;
const BVHModel<RSS>* model1 = (const BVHModel<RSS>*)o1;
const BVHModel<RSS>* model2 = (const BVHModel<RSS>*)o2;
- initialize(node, *model1, *model2, R1_1, T1_1, R2_1, T2_1);
+ // whether the first start configuration is in collision
+ MeshCollisionTraversalNodeRSS cnode;
+ if(!initialize(cnode, *model1, *model2))
+ std::cout << "initialize error" << std::endl;
+
+ relativeTransform(R1_1, T1_1, R2_1, T2_1, cnode.R, cnode.T);
+
+ cnode.enable_statistics = false;
+ cnode.num_max_contacts = num_max_contacts;
+ cnode.exhaustive = exhaustive;
+ cnode.enable_contact = enable_contact;
- node.motion1 = new InterpMotion<RSS>(R1_1, T1_1, R1_2, T1_2);
- node.motion2 = new InterpMotion<RSS>(R2_1, T2_1, R2_2, T2_2);
+ collide(&cnode);
- int b = collide(o1, o2, num_max_contacts, exhaustive, enable_contact, contacts);
+ int b = cnode.pairs.size();
if(b > 0)
{
toc = 0;
+ // std::cout << "zero collide" << std::endl;
return b;
}
+
+ MeshConservativeAdvancementTraversalNodeRSS node;
+
+ initialize(node, *model1, *model2, R1_1, T1_1, R2_1, T2_1);
+
+ InterpMotion<RSS> motion1(R1_1, T1_1, R1_2, T1_2, O1);
+ InterpMotion<RSS> motion2(R2_1, T2_1, R2_2, T2_2, O2);
+
+ node.motion1 = &motion1;
+ node.motion2 = &motion2;
+
+ int iter = 0;
+
do
{
+ iter++;
Vec3f R1_t[3];
Vec3f R2_t[3];
Vec3f T1_t;
@@ -111,7 +136,10 @@ int conservativeAdvancement(const CollisionObject* o1,
distanceRecurse(&node, 0, 0, NULL);
if(node.delta_t < node.t_err)
+ {
+ // std::cout << node.delta_t << " " << node.t_err << std::endl;
break;
+ }
node.toc += node.delta_t;
@@ -126,6 +154,10 @@ int conservativeAdvancement(const CollisionObject* o1,
}
while(1);
+ std::cout << iter << std::endl;
+
+ toc = node.toc;
+
if(node.toc < 1)
return 1;
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 0abc93221770b001bd611188dbf95ba9c827f064..91c274ce75e4da6b3de640341ad58ca32842ea1c 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -421,7 +421,10 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
BVH_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
BVH_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
- BVH_REAL cur_delta_t = c / (bound1 + bound2);
+ BVH_REAL bound = bound1 + bound2;
+ if(bound < c) bound = c;
+
+ BVH_REAL cur_delta_t = c / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
@@ -476,7 +479,10 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
BVH_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
BVH_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
- BVH_REAL cur_delta_t = c / (bound1 + bound2);
+ BVH_REAL bound = bound1 + bound2;
+ if(bound < c) bound = c;
+
+ BVH_REAL cur_delta_t = c / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
@@ -616,7 +622,10 @@ bool MeshConservativeAdvancementTraversalNodeRSS::canStop(BVH_REAL c) const
BVH_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
BVH_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
- BVH_REAL cur_delta_t = c / (bound1 + bound2);
+ BVH_REAL bound = bound1 + bound2;
+ if(bound < d) bound = d;
+
+ BVH_REAL cur_delta_t = c / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
diff --git a/trunk/fcl/test/test_core_conservative_advancement.cpp b/trunk/fcl/test/test_core_conservative_advancement.cpp
index 5c999ea76481e2e21cf8095805b9902a4f3450ab..4d86fe208eebc58f14eb9e9403da8bf4c9c9a784 100644
--- a/trunk/fcl/test/test_core_conservative_advancement.cpp
+++ b/trunk/fcl/test/test_core_conservative_advancement.cpp
@@ -54,8 +54,7 @@ bool interp_ccd_Test(const Transform& tf1, const Transform& tf2,
SplitMethodType split_method,
unsigned int nsamples);
-
-void computeInterpTransform(const Transform& tf1, const Transform& tf2, BVH_REAL t, Transform& tf);
+unsigned int n_dcd_samples = 10;
int main()
{
@@ -74,7 +73,18 @@ int main()
for(unsigned int i = 0; i < transforms.size(); ++i)
{
- CA_ccd_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
+ std::cout << i << std::endl;
+ bool res = CA_ccd_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN);
+ if(res) std::cout << "yes"; else std::cout << "no";
+ std::cout << std::endl;
+
+ std::cout << "-----------" << std::endl;
+ bool res2 = interp_ccd_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples);
+
+ if(res2) std::cout << "yes"; else std::cout << "no";
+ std::cout << std::endl;
+
+ std::cout << std::endl;
}
return 1;
@@ -110,8 +120,8 @@ bool CA_ccd_Test(const Transform& tf1, const Transform& tf2,
std::vector<Contact> contacts;
BVH_REAL toc;
- int b = conservativeAdvancement(&m1, tf1.R, tf1.T, tf2.R, tf2.T,
- &m2, R2, T2, R2, T2,
+ int b = conservativeAdvancement(&m1, tf1.R, tf1.T, tf2.R, tf2.T, Vec3f(),
+ &m2, R2, T2, R2, T2, Vec3f(),
1, false, false, contacts, toc);
std::cout << "t " << toc << std::endl;
@@ -119,69 +129,6 @@ bool CA_ccd_Test(const Transform& tf1, const Transform& tf2,
return (b > 0);
}
-void computeInterpTransform(const Transform& tf1, const Transform& tf2, BVH_REAL t, Transform& tf)
-{
- Vec3f T_t = tf1.T + (tf2.T - tf1.T) * t;
- Vec3f R_t[3];
-
- Vec3f M[3];
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- for(int k = 0; k < 3; ++k)
- M[i][j] += tf2.R[i][k] * tf1.R[j][k];
- }
-
- SimpleQuaternion q;
- q.fromRotation(M);
-
- Vec3f axis;
- BVH_REAL angle;
- q.toAxisAngle(axis, angle);
-
- Vec3f A[3];
- Vec3f B[3];
- Vec3f C[3];
-
- Vec3f tmp[3];
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- if(i == j)
- tmp[i][j] = 1 - axis[i] * axis[j];
- else
- tmp[i][j] = 0 - axis[i] * axis[j];
- }
- }
-
- matMulMat(tmp, tf1.R, A);
-
- tmp[0][0] = 0; tmp[0][1] = -axis[2]; tmp[0][2] = axis[1];
- tmp[1][0] = axis[2]; tmp[1][1] = 0; tmp[1][2] = -axis[0];
- tmp[2][0] = -axis[1]; tmp[2][1] = axis[0]; tmp[2][2] = 0;
-
- matMulMat(tmp, tf1.R, B);
-
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- tmp[i][j] = axis[i] * axis[j];
- }
- }
-
- matMulMat(tmp, tf1.R, C);
-
- for(int i = 0; i < 3; ++i)
- {
- R_t[i] = A[i] * cos(angle * t) + B[i] * sin(angle * t) + C[i];
- }
-
- tf.T = T_t;
- for(int i = 0; i < 3; ++i)
- tf.R[i] = R_t[i];
-}
bool interp_ccd_Test(const Transform& tf1, const Transform& tf2,
const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
@@ -210,15 +157,23 @@ bool interp_ccd_Test(const Transform& tf1, const Transform& tf2,
R2[1] = Vec3f(0, 1, 0);
R2[2] = Vec3f(0, 0, 1);
+
+ InterpMotion<RSS> motion1(tf1.R, tf1.T, tf2.R, tf2.T, Vec3f());
+
for(unsigned int i = 0; i <= nsamples; ++i)
{
- BVH_REAL delta = i / (BVH_REAL)nsamples;
+ BVH_REAL curt = i / (BVH_REAL)nsamples;
+
+ Vec3f R[3];
+ Vec3f T;
+ motion1.integrate(curt);
+ motion1.getCurrentTransform(R, T);
- Transform tf;
- computeInterpTransform(tf1, tf2, delta, tf);
+ m1.setTransform(R, T);
+ m2.setTransform(R2, T2);
- MeshCollisionTraversalNode<RSS> node;
- if(!initialize(node, m1, m2))
+ MeshCollisionTraversalNodeRSS node;
+ if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
std::cout << "initialize error" << std::endl;
node.enable_statistics = false;
@@ -228,7 +183,11 @@ bool interp_ccd_Test(const Transform& tf1, const Transform& tf2,
collide(&node);
- if(node.pairs.size() > 0) return true;
+ if(node.pairs.size() > 0)
+ {
+ std::cout << "t " << curt << std::endl;
+ return true;
+ }
}
return false;