Commit 7fb021ba authored by jpan's avatar jpan
Browse files

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@54 253336fb-580f-4252-a368-f3cef5a2a82b
parent f53be82c
......@@ -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));
}
};
......
......@@ -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]);
}
};
}
......
......@@ -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;
......
......@@ -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);
......
......@@ -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];
......
......@@ -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;
};
......
......@@ -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
*/
......
......@@ -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)
{
......
......@@ -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();
......
......@@ -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
*/
......
......@@ -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;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment