Commit 77ab39da authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add non-recursive version of collisionRecurse (to be benchmarked).

parent fe5c1fbb
......@@ -62,6 +62,9 @@ void collisionRecurse(MeshCollisionTraversalNodeOBB* node, int b1, int b2, const
/// @brief Recurse function for collision, specialized for RSS type
void collisionRecurse(MeshCollisionTraversalNodeRSS* node, int b1, int b2, const Matrix3f& R, const Vec3f& T, BVHFrontList* front_list);
void collisionNonRecurse(CollisionTraversalNodeBase* node,
BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound);
/// @brief Recurse function for distance
void distanceRecurse(DistanceTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
......
......@@ -38,6 +38,8 @@
#include <hpp/fcl/traversal/traversal_recurse.h>
#include <vector>
namespace hpp
{
namespace fcl
......@@ -103,6 +105,72 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
throw std::runtime_error ("Not implemented.");
}
void collisionNonRecurse(CollisionTraversalNodeBase* node,
BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)
{
typedef std::pair<int, int> BVPair_t;
//typedef std::stack<BVPair_t, std::vector<BVPair_t> > Stack_t;
typedef std::vector<BVPair_t> Stack_t;
Stack_t pairs;
pairs.reserve (1000);
sqrDistLowerBound = std::numeric_limits<FCL_REAL>::infinity();
FCL_REAL sdlb = std::numeric_limits<FCL_REAL>::infinity();
pairs.push_back (BVPair_t (0, 0));
while (!pairs.empty()) {
int a = pairs.back().first,
b = pairs.back().second;
pairs.pop_back();
bool la = node->isFirstNodeLeaf(a);
bool lb = node->isSecondNodeLeaf(b);
// Leaf / Leaf case
if (la && lb) {
updateFrontList(front_list, a, b);
// TODO should we test the BVs ?
//if(node->BVTesting(a, b, sdlb)) {
//if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
//continue;
//}
node->leafTesting(a, b, sdlb);
if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
if (node->canStop() && !front_list) return;
continue;
}
// TODO shouldn't we test the leaf triangle against BV is la != lb
// if (la && !lb) { // leaf triangle 1 against BV 2
// } else if (!la && lb) { // BV 1 against leaf triangle 2
// }
// Check the BV
if(node->BVTesting(a, b, sdlb)) {
if (sdlb < sqrDistLowerBound) sqrDistLowerBound = sdlb;
updateFrontList(front_list, a, b);
continue;
}
if(node->firstOverSecond(a, b))
{
int c1 = node->getFirstLeftChild(a);
int c2 = node->getFirstRightChild(a);
pairs.push_back (BVPair_t (c2, b));
pairs.push_back (BVPair_t (c1, b));
}
else
{
int c1 = node->getSecondLeftChild(b);
int c2 = node->getSecondRightChild(b);
pairs.push_back (BVPair_t (a, c2));
pairs.push_back (BVPair_t (a, c1));
}
}
}
/** Recurse function for self collision
* Make sure node is set correctly so that the first and second tree are the same
*/
......
Supports Markdown
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