diff --git a/trunk/fcl/include/fcl/BVH_model.h b/trunk/fcl/include/fcl/BVH_model.h
index 2eef52b66ee12ef9985a508164b64550e740fcae..a1489bb0590eb4d5b16e132d95301818c9f8d06b 100644
--- a/trunk/fcl/include/fcl/BVH_model.h
+++ b/trunk/fcl/include/fcl/BVH_model.h
@@ -223,6 +223,11 @@ public:
/** \brief Check the number of memory used */
int memUsage(int msg) const;
+ void makeParentRelative()
+ {
+ makeParentRelativeRecurse(0, Matrix3f::getIdentity(), Vec3f());
+ }
+
private:
/** \brief Bounding volume hierarchy */
@@ -249,6 +254,23 @@ private:
/** \brief Recursive kernel for bottomup refitting */
int recursiveRefitTree_bottomup(int bv_id);
+ void makeParentRelativeRecurse(int bv_id, const Matrix3f& parentR, const Vec3f& parentT)
+ {
+ if(!bvs[bv_id].isLeaf())
+ {
+ makeParentRelativeRecurse(bvs[bv_id].first_child, bvs[bv_id].getOrientation(), bvs[bv_id].getCenter());
+
+ makeParentRelativeRecurse(bvs[bv_id].first_child + 1, bvs[bv_id].getOrientation(), bvs[bv_id].getCenter());
+ }
+
+ // make self parent relative
+ Matrix3f Rpc = parentR.transposeTimes(bvs[bv_id].getOrientation());
+ bvs[bv_id].setOrientation(Rpc);
+
+ Vec3f Tpc = bvs[bv_id].getCenter() - parentT;
+ bvs[bv_id].setCenter(parentR.transposeTimes(Tpc));
+ }
+
};
diff --git a/trunk/fcl/include/fcl/BV_node.h b/trunk/fcl/include/fcl/BV_node.h
index 26268e04734f2c0e63a36cb6c37766cf4ca2875e..0058275a7919a88e5ea6df9cc1b6ad376b87393a 100644
--- a/trunk/fcl/include/fcl/BV_node.h
+++ b/trunk/fcl/include/fcl/BV_node.h
@@ -38,17 +38,18 @@
#ifndef FCL_BV_NODE_H
#define FCL_BV_NODE_H
+#include "fcl/vec_3f.h"
+#include "fcl/matrix_3f.h"
+
+#include "fcl/OBB.h"
+#include "fcl/RSS.h"
+
/** \brief Main namespace */
namespace fcl
{
-/** \brief A class describing a bounding volume node */
-template<typename BV>
-struct BVNode
+struct BVNodeBase
{
- /** \brief A bounding volume */
- BV bv;
-
/** \brief An index for first child node or primitive
* If the value is positive, it is the index of the first child bv node
* If the value is negative, it is -(primitive index + 1)
@@ -65,16 +66,24 @@ struct BVNode
int num_primitives;
/** \brief Whether current node is a leaf node (i.e. contains a primitive index */
- bool isLeaf() const { return first_child < 0; }
+ inline bool isLeaf() const { return first_child < 0; }
/** \brief Return the primitive index. The index is referred to the original data (i.e. vertices or tri_indices) in BVHModel */
- int primitiveId() const { return -(first_child + 1); }
+ inline int primitiveId() const { return -(first_child + 1); }
/** \brief Return the index of the first child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel */
- int leftChild() const { return first_child; }
+ inline int leftChild() const { return first_child; }
/** \brief Return the index of the second child. The index is referred to the bounding volume array (i.e. bvs) in BVHModel */
- int rightChild() const { return first_child + 1; }
+ inline int rightChild() const { return first_child + 1; }
+};
+
+/** \brief A class describing a bounding volume node */
+template<typename BV>
+struct BVNode : public BVNodeBase
+{
+ /** \brief A bounding volume */
+ BV bv;
/** \brief Check whether two BVNode collide */
bool overlap(const BVNode& other) const
@@ -87,6 +96,79 @@ struct BVNode
return bv.distance(other.bv, P1, P2);
}
+ Vec3f getCenter() const { return Vec3f(); }
+ void setCenter(const Vec3f& v) {}
+ Matrix3f getOrientation() const { return Matrix3f::getIdentity(); }
+ void setOrientation(const Matrix3f& m) {}
+};
+
+template<>
+struct BVNode<OBB> : public BVNodeBase
+{
+ /** \brief A bounding volume */
+ OBB bv;
+
+ /** \brief Check whether two BVNode collide */
+ bool overlap(const BVNode& other) const
+ {
+ return bv.overlap(other.bv);
+ }
+
+ BVH_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
+ {
+ return bv.distance(other.bv, P1, P2);
+ }
+
+ Vec3f getCenter() const { return bv.To; }
+ void setCenter(const Vec3f& v) { bv.To = v; }
+ Matrix3f getOrientation() const
+ {
+ Matrix3f m(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
+ return m;
+ }
+ void setOrientation(const Matrix3f& m)
+ {
+ bv.axis[0].setValue(m[0][0], m[1][0], m[2][0]);
+ bv.axis[1].setValue(m[0][1], m[1][1], m[2][1]);
+ bv.axis[2].setValue(m[0][2], m[1][2], m[2][2]);
+ }
+};
+
+template<>
+struct BVNode<RSS> : public BVNodeBase
+{
+ /** \brief A bounding volume */
+ RSS bv;
+
+ /** \brief Check whether two BVNode collide */
+ bool overlap(const BVNode& other) const
+ {
+ return bv.overlap(other.bv);
+ }
+
+ BVH_REAL distance(const BVNode& other, Vec3f* P1 = NULL, Vec3f* P2 = NULL) const
+ {
+ return bv.distance(other.bv, P1, P2);
+ }
+
+ Vec3f getCenter() const { return bv.Tr; }
+ void setCenter(const Vec3f& v) { bv.Tr = v; }
+ Matrix3f getOrientation() const
+ {
+ Matrix3f m(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
+ return m;
+ }
+
+ void setOrientation(const Matrix3f& m)
+ {
+ bv.axis[0].setValue(m[0][0], m[1][0], m[2][0]);
+ bv.axis[1].setValue(m[0][1], m[1][1], m[2][1]);
+ bv.axis[2].setValue(m[0][2], m[1][2], m[2][2]);
+ }
};
}
diff --git a/trunk/fcl/include/fcl/RSS.h b/trunk/fcl/include/fcl/RSS.h
index 08c069b27092baee871132038a51a20bd749f8d0..61c435731928ed552cc95e56bb5e44dd82a1839a 100644
--- a/trunk/fcl/include/fcl/RSS.h
+++ b/trunk/fcl/include/fcl/RSS.h
@@ -126,6 +126,7 @@ public:
return Tr;
}
+
/** \brief the distance between two RSS */
BVH_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
diff --git a/trunk/fcl/include/fcl/collision_node.h b/trunk/fcl/include/fcl/collision_node.h
index 00d3137707bef565fbb6035cb46a5b002fef1e77..29f8a46cf585ea71aab0b747a8a26229e2e4a3b6 100644
--- a/trunk/fcl/include/fcl/collision_node.h
+++ b/trunk/fcl/include/fcl/collision_node.h
@@ -47,6 +47,10 @@ namespace fcl
void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);
+void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list = NULL);
+
+void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list = NULL);
+
void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list = NULL);
void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL, int qsize = 2);
diff --git a/trunk/fcl/include/fcl/matrix_3f.h b/trunk/fcl/include/fcl/matrix_3f.h
index 88ef63a11747228ab9d3338999b657e23c4db500..c72b61fe097179f6cc61053c7a8fb10f854a4d3b 100644
--- a/trunk/fcl/include/fcl/matrix_3f.h
+++ b/trunk/fcl/include/fcl/matrix_3f.h
@@ -58,6 +58,13 @@ namespace fcl
zx, zy, zz);
}
+ Matrix3f(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3)
+ {
+ v_[0] = v1;
+ v_[1] = v2;
+ v_[2] = v3;
+ }
+
Matrix3f(const Matrix3f& other)
{
v_[0] = other.v_[0];
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index 726b5f073f0de9901b19deaf994e7f2cc6d95a03..84f999361f11f14787a08c6c13b1bf4004833fce 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -275,6 +275,10 @@ public:
void leafTesting(int b1, int b2) const;
+ bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
+
+ void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
+
Matrix3f R;
Vec3f T;
};
@@ -289,6 +293,10 @@ public:
void leafTesting(int b1, int b2) const;
+ bool BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
+
+ void leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const;
+
Matrix3f R;
Vec3f T;
};
diff --git a/trunk/fcl/include/fcl/traversal_recurse.h b/trunk/fcl/include/fcl/traversal_recurse.h
index ca5838835619ee1db43e400cb328f9b298ee27e0..604258a104c09cd645a25895a399430f9d4712f4 100644
--- a/trunk/fcl/include/fcl/traversal_recurse.h
+++ b/trunk/fcl/include/fcl/traversal_recurse.h
@@ -39,6 +39,7 @@
#define FCL_TRAVERSAL_RECURSE_H
#include "fcl/traversal_node_base.h"
+#include "fcl/traversal_node_bvhs.h"
#include "fcl/BVH_front.h"
#include <queue>
@@ -52,6 +53,9 @@ inline void updateFrontList(BVHFrontList* front_list, int b1, int b2)
void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
+void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list);
+void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list);
+
/** Recurse function for self collision
* Make sure node is set correctly so that the first and second tree are the same
*/
diff --git a/trunk/fcl/src/collision_node.cpp b/trunk/fcl/src/collision_node.cpp
index 3bd1b07125f8bbb42539eb3a8a34bdfe5012fe59..70d1775fb87a241d2d8cb2c8d351302be95cc1b9 100644
--- a/trunk/fcl/src/collision_node.cpp
+++ b/trunk/fcl/src/collision_node.cpp
@@ -53,6 +53,39 @@ void collide(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
}
}
+void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list)
+{
+ if(front_list && front_list->size() > 0)
+ {
+ propagateBVHFrontListCollisionRecurse(node, front_list);
+ }
+ else
+ {
+ Matrix3f Rtemp, R;
+ Vec3f Ttemp, T;
+ Rtemp = node->R * node->model2->getBV(0).getOrientation();
+ R = node->model1->getBV(0).getOrientation().transposeTimes(Rtemp);
+ Ttemp = node->R * node->model2->getBV(0).getCenter() + node->T;
+ Ttemp -= node->model1->getBV(0).getCenter();
+ T = node->model1->getBV(0).getOrientation().transposeTimes(Ttemp);
+
+ collisionRecurse(node, 0, 0, R, T, front_list);
+ }
+}
+
+void collide2(MeshCollisionTraversalNodeRSS* node, BVHFrontList* front_list)
+{
+ if(front_list && front_list->size() > 0)
+ {
+ propagateBVHFrontListCollisionRecurse(node, front_list);
+ }
+ else
+ {
+ collisionRecurse(node, 0, 0, node->R, node->T, front_list);
+ }
+}
+
+
void selfCollide(CollisionTraversalNodeBase* node, BVHFrontList* front_list)
{
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 6c00681e02fa65baafc15fd9954effd475662d64..32bcccdfdb9a348d009c2042afbe72790a61d364 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -102,6 +102,63 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
}
+bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const
+{
+ if(enable_statistics) num_bv_tests++;
+ return OBB::obbDisjoint(Rc, Tc, model1->getBV(b1).bv.extent, model2->getBV(b2).bv.extent);
+}
+
+void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const
+{
+ if(enable_statistics) num_leaf_tests++;
+
+ const BVNode<OBB>& node1 = model1->getBV(b1);
+ const BVNode<OBB>& 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));
+ }
+ }
+ }
+}
+
+
+
MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS() : MeshCollisionTraversalNode<RSS>()
{
R.setIdentity();
diff --git a/trunk/fcl/src/traversal_recurse.cpp b/trunk/fcl/src/traversal_recurse.cpp
index 8c0f2f7a2ddc18a535917e5f63383523d57a8376..590e61b1eadf4e0579ae640712e6dceaa1c0af9b 100644
--- a/trunk/fcl/src/traversal_recurse.cpp
+++ b/trunk/fcl/src/traversal_recurse.cpp
@@ -86,6 +86,91 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFront
}
}
+void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list)
+{
+ bool l1 = node->isFirstNodeLeaf(b1);
+ bool l2 = node->isSecondNodeLeaf(b2);
+
+ if(l1 && l2)
+ {
+ updateFrontList(front_list, b1, b2);
+
+ if(node->BVTesting(b1, b2, R, T)) return;
+
+ node->leafTesting(b1, b2, R, T);
+ return;
+ }
+
+ if(node->BVTesting(b1, b2, R, T))
+ {
+ updateFrontList(front_list, b1, b2);
+ return;
+ }
+
+ Vec3f temp;
+
+ if(node->firstOverSecond(b1, b2))
+ {
+ int c1 = node->getFirstLeftChild(b1);
+ int c2 = node->getFirstRightChild(b1);
+
+ const OBB& bv1 = node->model1->getBV(c1).bv;
+
+ Matrix3f Rc(R.transposeTimes(bv1.axis[0]), R.transposeTimes(bv1.axis[1]), R.transposeTimes(bv1.axis[2]));
+ temp = T - bv1.To;
+ Vec3f Tc(temp.dot(bv1.axis[0]), temp.dot(bv1.axis[1]), temp.dot(bv1.axis[2]));
+
+ collisionRecurse(node, c1, b2, Rc, Tc, front_list);
+
+ // early stop is disabled is front_list is used
+ if(node->canStop() && !front_list) return;
+
+ const OBB& bv2 = node->model1->getBV(c2).bv;
+
+ Rc = Matrix3f(R.transposeTimes(bv2.axis[0]), R.transposeTimes(bv2.axis[1]), R.transposeTimes(bv2.axis[2]));
+ temp = T - bv2.To;
+ Tc.setValue(temp.dot(bv2.axis[0]), temp.dot(bv2.axis[1]), temp.dot(bv2.axis[2]));
+
+ collisionRecurse(node, c2, b2, Rc, Tc, front_list);
+ }
+ else
+ {
+ int c1 = node->getSecondLeftChild(b2);
+ int c2 = node->getSecondRightChild(b2);
+
+ const OBB& bv1 = node->model2->getBV(c1).bv;
+ Matrix3f Rc;
+ temp = R * bv1.axis[0];
+ Rc[0][0] = temp[0]; Rc[1][0] = temp[1]; Rc[2][0] = temp[2];
+ temp = R * bv1.axis[1];
+ Rc[0][1] = temp[0]; Rc[1][1] = temp[1]; Rc[2][1] = temp[2];
+ temp = R * bv1.axis[2];
+ Rc[0][2] = temp[0]; Rc[1][2] = temp[1]; Rc[2][2] = temp[2];
+ Vec3f Tc = R * bv1.To + T;
+
+ collisionRecurse(node, b1, c1, Rc, Tc, front_list);
+
+ // early stop is disabled is front_list is used
+ if(node->canStop() && !front_list) return;
+
+ const OBB& bv2 = node->model2->getBV(c2).bv;
+ temp = R * bv2.axis[0];
+ Rc[0][0] = temp[0]; Rc[1][0] = temp[1]; Rc[2][0] = temp[2];
+ temp = R * bv2.axis[1];
+ Rc[0][1] = temp[0]; Rc[1][1] = temp[1]; Rc[2][1] = temp[2];
+ temp = R * bv2.axis[2];
+ Rc[0][2] = temp[0]; Rc[1][2] = temp[1]; Rc[2][2] = temp[2];
+ Tc = R * bv2.To + T;
+
+ collisionRecurse(node, b1, c2, Rc, Tc, front_list);
+ }
+}
+
+void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list)
+{
+
+}
+
/** Recurse function for self collision
* Make sure node is set correctly so that the first and second tree are the same
*/
diff --git a/trunk/fcl/test/timing_test.cpp b/trunk/fcl/test/timing_test.cpp
index f52fce2d52ec7b43fcf0ec1fb64fd157b4ef5861..051ff046c7bc68ea9df030945b6a0c06e9faf2a2 100644
--- a/trunk/fcl/test/timing_test.cpp
+++ b/trunk/fcl/test/timing_test.cpp
@@ -62,6 +62,59 @@ int main()
generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
+ std::cout << "FCL timing 2" << std::endl;
+ {
+ double t_fcl = 0;
+
+ BVHModel<OBB> m1;
+ BVHModel<OBB> m2;
+ SplitMethodType split_method = SPLIT_METHOD_MEAN;
+ 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();
+ m1.makeParentRelative();
+
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+ m2.makeParentRelative();
+
+
+ Matrix3f R2;
+ R2.setIdentity();
+ Vec3f T2;
+
+ for(unsigned int i = 0; i < transforms.size(); ++i)
+ {
+ Transform& tf = transforms[i];
+ 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 = false;
+ node.num_max_contacts = -1;
+ node.exhaustive = false;
+ node.enable_contact = false;
+
+ Timer timer;
+ timer.start();
+ collide2(&node);
+ timer.stop();
+ t_fcl += timer.getElapsedTime();
+
+ //std::cout << node.pairs.size() << std::endl;
+ }
+
+ std::cout << "fcl timing " << t_fcl << " ms" << std::endl;
+ }
+
+
std::cout << "PQP timing" << std::endl;
#if USE_PQP
{
@@ -89,6 +142,7 @@ int main()
}
m1.EndModel();
+
m2.BeginModel();
for(unsigned int i = 0; i < triangles2.size(); ++i)
{
@@ -191,6 +245,7 @@ int main()
}
+
return 1;
}