diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h
index 5e9dd63c0a34c2a8fdac4f753f243e2b56daaaef..2a943fa2855324f65e95dbd4ffef04542ffdcd46 100644
--- a/include/fcl/traversal/traversal_node_bvhs.h
+++ b/include/fcl/traversal/traversal_node_bvhs.h
@@ -662,7 +662,8 @@ public:
struct ConservativeAdvancementStackData
{
- ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_) : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
+ ConservativeAdvancementStackData(const Vec3f& P1_, const Vec3f& P2_, int c1_, int c2_, FCL_REAL d_)
+ : P1(P1_), P2(P2_), c1(c1_), c2(c2_), d(d_) {}
Vec3f P1;
Vec3f P2;
@@ -846,6 +847,9 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const;
template<>
bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const;
+template<>
+bool MeshConservativeAdvancementTraversalNode<OBBRSS>::canStop(FCL_REAL c) const;
+
class MeshConservativeAdvancementTraversalNodeRSS : public MeshConservativeAdvancementTraversalNode<RSS>
{
@@ -862,6 +866,20 @@ public:
Vec3f T;
};
+class MeshConservativeAdvancementTraversalNodeOBBRSS : public MeshConservativeAdvancementTraversalNode<OBBRSS>
+{
+public:
+ MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_ = 1);
+
+ FCL_REAL BVTesting(int b1, int b2) const;
+
+ void leafTesting(int b1, int b2) const;
+
+ bool canStop(FCL_REAL c) const;
+
+ Matrix3f R;
+ Vec3f T;
+};
}
diff --git a/include/fcl/traversal/traversal_node_setup.h b/include/fcl/traversal/traversal_node_setup.h
index b9ead423c3df35a5a2c71bef92233f7a0e46d868..fd73e34bea4291f364e2dd82ea842c46a9bfec26 100644
--- a/include/fcl/traversal/traversal_node_setup.h
+++ b/include/fcl/traversal/traversal_node_setup.h
@@ -1048,9 +1048,9 @@ bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms
template<typename BV>
bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
- BVHModel<BV>& model1,
- BVHModel<BV>& model2,
- const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1,
+ BVHModel<BV>& model1, const Transform3f& tf1,
+ BVHModel<BV>& model2, const Transform3f& tf2,
+ FCL_REAL w = 1,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
@@ -1060,7 +1060,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
- Vec3f new_v = R1 * p + T1;
+ Vec3f new_v = tf1.transform(p);
vertices_transformed1[i] = new_v;
}
@@ -1069,7 +1069,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
- Vec3f new_v = R2 * p + T2;
+ Vec3f new_v = tf2.transform(p);
vertices_transformed2[i] = new_v;
}
@@ -1097,27 +1097,15 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node,
/// @brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS
-inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
- const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, FCL_REAL w = 1)
-{
- if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
- return false;
-
- node.model1 = &model1;
- node.model2 = &model2;
-
- node.vertices1 = model1.vertices;
- node.vertices2 = model2.vertices;
-
- node.tri_indices1 = model1.tri_indices;
- node.tri_indices2 = model2.tri_indices;
-
- node.w = w;
-
- relativeTransform(R1, T1, R2, T2, node.R, node.T);
+bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node,
+ const BVHModel<RSS>& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
+ FCL_REAL w = 1);
- return true;
-}
+bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node,
+ const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
+ const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
+ FCL_REAL w = 1);
}
diff --git a/src/ccd/conservative_advancement.cpp b/src/ccd/conservative_advancement.cpp
index 298397b762ec4d888ca4b31c9fee262b1cde8c91..8899a256c89e76d678c01dcaa064a4d230064697 100644
--- a/src/ccd/conservative_advancement.cpp
+++ b/src/ccd/conservative_advancement.cpp
@@ -106,7 +106,7 @@ int conservativeAdvancement(const CollisionGeometry* o1,
MeshConservativeAdvancementTraversalNodeRSS node;
- initialize(node, *model1, *model2, tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation());
+ initialize(node, *model1, tf1, *model2, tf2);
node.motion1 = motion1;
node.motion2 = motion2;
diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp
index b9f8a9d6068c3c0a1f3a92718af0dbee87990943..2c1eb5ea235dc87b49c571579db3c462b671196a 100644
--- a/src/traversal/traversal_node_bvhs.cpp
+++ b/src/traversal/traversal_node_bvhs.cpp
@@ -428,11 +428,33 @@ void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
}
-/// for OBB and RSS, there is local coordinate of BV, so normal need to be transformed
+
+namespace details
+{
+
+template<typename BV>
+const Vec3f& getBVAxis(const BV& bv, int i)
+{
+ return bv.axis[i];
+}
+
template<>
-bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
+const Vec3f& getBVAxis<OBBRSS>(const OBBRSS& bv, int i)
{
- if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
+ return bv.obb.axis[i];
+}
+
+
+template<typename BV>
+bool meshConservativeAdvancementTraversalNodeCanStop(FCL_REAL c,
+ FCL_REAL min_distance,
+ FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
+ const BVHModel<BV>* model1, const BVHModel<BV>* model2,
+ const MotionBase<BV>* motion1, const MotionBase<BV>* motion2,
+ std::vector<ConservativeAdvancementStackData>& stack,
+ FCL_REAL& delta_t)
+{
+ if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
FCL_REAL d = data.d;
@@ -457,10 +479,13 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
assert(c == d);
- Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
+ Vec3f n_transformed =
+ getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
+ getBVAxis(model1->getBV(c1).bv, 1) * n[1] +
+ getBVAxis(model1->getBV(c1).bv, 2) * n[2];
- FCL_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
- FCL_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
+ FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
+ FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, n_transformed);
FCL_REAL bound = bound1 + bound2;
@@ -489,10 +514,53 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
}
}
+}
+
+/// for OBB, RSS and OBBRSS, there is local coordinate of BV, so normal need to be transformed
+template<>
+bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(FCL_REAL c) const
+{
+ return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance,
+ this->abs_err, this->rel_err, w,
+ this->model1, this->model2,
+ motion1, motion2,
+ stack, delta_t);
+}
+
template<>
bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const
{
- if((c >= w * (this->min_distance - this->abs_err)) && (c * (1 + this->rel_err) >= w * this->min_distance))
+ return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance,
+ this->abs_err, this->rel_err, w,
+ this->model1, this->model2,
+ motion1, motion2,
+ stack, delta_t);
+}
+
+template<>
+bool MeshConservativeAdvancementTraversalNode<OBBRSS>::canStop(FCL_REAL c) const
+{
+ return details::meshConservativeAdvancementTraversalNodeCanStop(c, this->min_distance,
+ this->abs_err, this->rel_err, w,
+ this->model1, this->model2,
+ motion1, motion2,
+ stack, delta_t);
+}
+
+
+namespace details
+{
+
+template<typename BV>
+bool meshConservativeAdvancementOrientedNodeCanStop(FCL_REAL c,
+ FCL_REAL min_distance,
+ FCL_REAL abs_err, FCL_REAL rel_err, FCL_REAL w,
+ const BVHModel<BV>* model1, const BVHModel<BV>* model2,
+ const MotionBase<BV>* motion1, const MotionBase<BV>* motion2,
+ std::vector<ConservativeAdvancementStackData>& stack,
+ FCL_REAL& delta_t)
+{
+ if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
{
const ConservativeAdvancementStackData& data = stack.back();
FCL_REAL d = data.d;
@@ -517,10 +585,18 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const
assert(c == d);
- Vec3f n_transformed = model1->getBV(c1).bv.axis[0] * n[0] + model1->getBV(c1).bv.axis[1] * n[1] + model1->getBV(c1).bv.axis[2] * n[2];
+ // n is in local frame of c1, so we need to turn n into the global frame
+ Vec3f n_transformed =
+ getBVAxis(model1->getBV(c1).bv, 0) * n[0] +
+ getBVAxis(model1->getBV(c1).bv, 1) * n[2] +
+ getBVAxis(model1->getBV(c1).bv, 2) * n[2];
+ Matrix3f R0;
+ motion1->getCurrentRotation(R0);
+ n_transformed = R0 * n_transformed;
+ n_transformed.normalize();
- FCL_REAL bound1 = motion1->computeMotionBound(this->model1->getBV(c1).bv, n_transformed);
- FCL_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
+ FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
+ FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
FCL_REAL bound = bound1 + bound2;
@@ -549,31 +625,24 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(FCL_REAL c) const
}
}
-
-MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_) : MeshConservativeAdvancementTraversalNode<RSS>(w_)
-{
- R.setIdentity();
- // default T is 0
-}
-
-FCL_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const
-{
- if(enable_statistics) num_bv_tests++;
- Vec3f P1, P2;
- FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
-
- stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
-
- return d;
-}
-
-
-void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const
+template<typename BV>
+void meshConservativeAdvancementOrientedNodeLeafTesting(int b1, int b2,
+ const BVHModel<BV>* model1, const BVHModel<BV>* model2,
+ const Triangle* tri_indices1, const Triangle* tri_indices2,
+ const Vec3f* vertices1, const Vec3f* vertices2,
+ const Matrix3f& R, const Vec3f& T,
+ const MotionBase<BV>* motion1, const MotionBase<BV>* motion2,
+ bool enable_statistics,
+ FCL_REAL& min_distance,
+ Vec3f& p1, Vec3f& p2,
+ int& last_tri_id1, int& last_tri_id2,
+ FCL_REAL& delta_t,
+ int& num_leaf_tests)
{
if(enable_statistics) num_leaf_tests++;
- const BVNode<RSS>& node1 = model1->getBV(b1);
- const BVNode<RSS>& node2 = model2->getBV(b2);
+ const BVNode<BV>& node1 = model1->getBV(b1);
+ const BVNode<BV>& node2 = model2->getBV(b2);
int primitive_id1 = node1.primitiveId();
int primitive_id2 = node2.primitiveId();
@@ -628,68 +697,100 @@ void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) co
delta_t = cur_delta_t;
}
-bool MeshConservativeAdvancementTraversalNodeRSS::canStop(FCL_REAL c) const
+}
+
+
+MeshConservativeAdvancementTraversalNodeRSS::MeshConservativeAdvancementTraversalNodeRSS(FCL_REAL w_)
+ : MeshConservativeAdvancementTraversalNode<RSS>(w_)
{
- if((c >= w * (min_distance - abs_err)) && (c * (1 + rel_err) >= w * min_distance))
- {
- const ConservativeAdvancementStackData& data = stack.back();
- FCL_REAL d = data.d;
- Vec3f n;
- int c1, c2;
+ R.setIdentity();
+}
- if(d > c)
- {
- const ConservativeAdvancementStackData& data2 = stack[stack.size() - 2];
- d = data2.d;
- n = data2.P2 - data2.P1;
- c1 = data2.c1;
- c2 = data2.c2;
- stack[stack.size() - 2] = stack[stack.size() - 1];
- }
- else
- {
- n = data.P2 - data.P1;
- c1 = data.c1;
- c2 = data.c2;
- }
+FCL_REAL MeshConservativeAdvancementTraversalNodeRSS::BVTesting(int b1, int b2) const
+{
+ if(enable_statistics) num_bv_tests++;
+ Vec3f P1, P2;
+ FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
- assert(c == d);
+ stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
- // 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];
- Matrix3f R0;
- motion1->getCurrentRotation(R0);
- n_transformed = R0 * n_transformed;
- n_transformed.normalize();
+ return d;
+}
- FCL_REAL bound1 = motion1->computeMotionBound(model1->getBV(c1).bv, n_transformed);
- FCL_REAL bound2 = motion2->computeMotionBound(model2->getBV(c2).bv, -n_transformed);
- FCL_REAL bound = bound1 + bound2;
+void MeshConservativeAdvancementTraversalNodeRSS::leafTesting(int b1, int b2) const
+{
+ details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2,
+ model1, model2,
+ tri_indices1, tri_indices2,
+ vertices1, vertices2,
+ R, T,
+ motion1, motion2,
+ enable_statistics,
+ min_distance,
+ p1, p2,
+ last_tri_id1, last_tri_id2,
+ delta_t,
+ num_leaf_tests);
+}
- FCL_REAL cur_delta_t;
- if(bound <= c) cur_delta_t = 1;
- else cur_delta_t = c / bound;
+bool MeshConservativeAdvancementTraversalNodeRSS::canStop(FCL_REAL c) const
+{
+ return details::meshConservativeAdvancementOrientedNodeCanStop(c,
+ min_distance,
+ abs_err, rel_err, w,
+ model1, model2,
+ motion1, motion2,
+ stack,
+ delta_t);
+}
- if(cur_delta_t < delta_t)
- delta_t = cur_delta_t;
- stack.pop_back();
- return true;
- }
- else
- {
- const ConservativeAdvancementStackData& data = stack.back();
- FCL_REAL d = data.d;
- if(d > c)
- stack[stack.size() - 2] = stack[stack.size() - 1];
+MeshConservativeAdvancementTraversalNodeOBBRSS::MeshConservativeAdvancementTraversalNodeOBBRSS(FCL_REAL w_)
+ : MeshConservativeAdvancementTraversalNode<OBBRSS>(w_)
+{
+ R.setIdentity();
+}
- stack.pop_back();
+FCL_REAL MeshConservativeAdvancementTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
+{
+ if(enable_statistics) num_bv_tests++;
+ Vec3f P1, P2;
+ FCL_REAL d = distance(R, T, model1->getBV(b1).bv, model2->getBV(b2).bv, &P1, &P2);
- return false;
- }
+ stack.push_back(ConservativeAdvancementStackData(P1, P2, b1, b2, d));
+
+ return d;
+}
+
+
+void MeshConservativeAdvancementTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
+{
+ details::meshConservativeAdvancementOrientedNodeLeafTesting(b1, b2,
+ model1, model2,
+ tri_indices1, tri_indices2,
+ vertices1, vertices2,
+ R, T,
+ motion1, motion2,
+ enable_statistics,
+ min_distance,
+ p1, p2,
+ last_tri_id1, last_tri_id2,
+ delta_t,
+ num_leaf_tests);
+}
+
+bool MeshConservativeAdvancementTraversalNodeOBBRSS::canStop(FCL_REAL c) const
+{
+ return details::meshConservativeAdvancementOrientedNodeCanStop(c,
+ min_distance,
+ abs_err, rel_err, w,
+ model1, model2,
+ motion1, motion2,
+ stack,
+ delta_t);
}
diff --git a/src/traversal/traversal_node_setup.cpp b/src/traversal/traversal_node_setup.cpp
index 88fc4de565174ea6fe9a9718db19b5fb971687e4..bd83d00f757a85f507aa1f43e050d83628dd4373 100644
--- a/src/traversal/traversal_node_setup.cpp
+++ b/src/traversal/traversal_node_setup.cpp
@@ -178,5 +178,56 @@ bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
return details::setupMeshDistanceOrientedNode(node, model1, tf1, model2, tf2, request, result);
}
+namespace details
+{
+
+
+template<typename BV, typename OrientedDistanceNode>
+static inline bool setupMeshConservativeAdvancementOrientedDistanceNode(OrientedDistanceNode& node,
+ const BVHModel<BV>& model1, const Transform3f& tf1,
+ const BVHModel<BV>& model2, const Transform3f& tf2,
+ FCL_REAL w)
+{
+ if(model1.getModelType() != BVH_MODEL_TRIANGLES || model2.getModelType() != BVH_MODEL_TRIANGLES)
+ return false;
+
+ node.model1 = &model1;
+ node.model2 = &model2;
+
+ node.vertices1 = model1.vertices;
+ node.vertices2 = model2.vertices;
+
+ node.tri_indices1 = model1.tri_indices;
+ node.tri_indices2 = model2.tri_indices;
+
+ node.w = w;
+
+ relativeTransform(tf1.getRotation(), tf1.getTranslation(), tf2.getRotation(), tf2.getTranslation(), node.R, node.T);
+
+ return true;
+}
+
+}
+
+
+bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node,
+ const BVHModel<RSS>& model1, const Transform3f& tf1,
+ const BVHModel<RSS>& model2, const Transform3f& tf2,
+ FCL_REAL w)
+{
+ return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w);
+}
+
+
+bool initialize(MeshConservativeAdvancementTraversalNodeOBBRSS& node,
+ const BVHModel<OBBRSS>& model1, const Transform3f& tf1,
+ const BVHModel<OBBRSS>& model2, const Transform3f& tf2,
+ FCL_REAL w)
+{
+ return details::setupMeshConservativeAdvancementOrientedDistanceNode(node, model1, tf1, model2, tf2, w);
+}
+
+
+
}