diff --git a/trunk/fcl/include/fcl/BV.h b/trunk/fcl/include/fcl/BV.h
index 9b84c478d82ac77c35603aacb89a920db8f9427a..94361a952aa83690aba8207d4764ac5312d567da 100644
--- a/trunk/fcl/include/fcl/BV.h
+++ b/trunk/fcl/include/fcl/BV.h
@@ -61,22 +61,6 @@ private:
}
};
-template<typename BV1>
-class Converter<BV1, AABB>
-{
-public:
- static void convert(const BV1& bv1, const SimpleTransform& tf1, AABB& bv2)
- {
- const Vec3f& center = bv1.center();
- FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5;
- Vec3f delta(r, r, r);
- Vec3f center2 = tf1.transform(center);
- bv2.min_ = center2 - delta;
- bv2.max_ = center2 + delta;
- }
-};
-
-
template<>
class Converter<AABB, AABB>
{
@@ -107,6 +91,72 @@ public:
}
};
+template<>
+class Converter<OBB, OBB>
+{
+public:
+ static void convert(const OBB& bv1, const SimpleTransform& tf1, OBB& bv2)
+ {
+ bv2.extent = bv1.extent;
+ bv2.To = tf1.transform(bv1.To);
+ bv2.axis[0] = tf1.transform(bv1.axis[0]);
+ bv2.axis[1] = tf1.transform(bv1.axis[1]);
+ bv2.axis[2] = tf1.transform(bv1.axis[2]);
+ }
+};
+
+template<>
+class Converter<OBBRSS, OBB>
+{
+public:
+ static void convert(const OBBRSS& bv1, const SimpleTransform& tf1, OBB& bv2)
+ {
+ Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
+ }
+};
+
+template<>
+class Converter<RSS, OBB>
+{
+public:
+ static void convert(const RSS& bv1, const SimpleTransform& tf1, OBB& bv2)
+ {
+ bv2.extent = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
+ bv2.To = tf1.transform(bv1.Tr);
+ bv2.axis[0] = tf1.transform(bv1.axis[0]);
+ bv2.axis[1] = tf1.transform(bv1.axis[1]);
+ bv2.axis[2] = tf1.transform(bv1.axis[2]);
+ }
+};
+
+
+template<typename BV1>
+class Converter<BV1, AABB>
+{
+public:
+ static void convert(const BV1& bv1, const SimpleTransform& tf1, AABB& bv2)
+ {
+ const Vec3f& center = bv1.center();
+ FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5;
+ Vec3f delta(r, r, r);
+ Vec3f center2 = tf1.transform(center);
+ bv2.min_ = center2 - delta;
+ bv2.max_ = center2 + delta;
+ }
+};
+
+template<typename BV1>
+class Converter<BV1, OBB>
+{
+public:
+ static void convert(const BV1& bv1, const SimpleTransform& tf1, OBB& bv2)
+ {
+ AABB bv;
+ Converter<BV1, AABB>::convert(bv1, SimpleTransform(), bv);
+ Converter<AABB, OBB>::convert(bv, tf1, bv2);
+ }
+};
+
diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/trunk/fcl/include/fcl/BV/AABB.h
index 248d2c1364d2f918066b9e2617bb4f5f2846a9cd..4370118fd6f32c69ac5d17715f55b62fd6e4e9ff 100644
--- a/trunk/fcl/include/fcl/BV/AABB.h
+++ b/trunk/fcl/include/fcl/BV/AABB.h
@@ -208,11 +208,17 @@ public:
min_ = min_ * ratio - core.min_;
max_ = max_ * ratio - core.max_;
return *this;
- }
-
-
+ }
};
+static inline AABB translate(const AABB& aabb, const Vec3f& t)
+{
+ AABB res(aabb);
+ res.min_ += t;
+ res.max_ += t;
+ return res;
+}
+
}
#endif
diff --git a/trunk/fcl/include/fcl/octree.h b/trunk/fcl/include/fcl/octree.h
index e598dbe2a332c5c5fc584778ef8966b8f42cdcaf..a88813b68a53ee955ed12a36f94f96dedca42c0b 100644
--- a/trunk/fcl/include/fcl/octree.h
+++ b/trunk/fcl/include/fcl/octree.h
@@ -64,6 +64,13 @@ public:
OcTree(FCL_REAL resolution) : tree(boost::shared_ptr<octomap::OcTree>(new octomap::OcTree(resolution))) {}
OcTree(const boost::shared_ptr<octomap::OcTree>& tree_) : tree(tree_) {}
+ void computeLocalAABB()
+ {
+ aabb_local = getRootBV();
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
+ }
+
inline AABB getRootBV() const
{
FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
@@ -109,11 +116,6 @@ public:
OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
- void computeLocalAABB()
- {
- aabb_center = Vec3f();
- aabb_radius = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
- }
};
static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv)
diff --git a/trunk/fcl/include/fcl/traversal_node_octree.h b/trunk/fcl/include/fcl/traversal_node_octree.h
index 41368bc5dc64be7c36d381110a1eeadac79b7688..a6dc192235242f987fcf2b4f851ae9b7d5016cd2 100644
--- a/trunk/fcl/include/fcl/traversal_node_octree.h
+++ b/trunk/fcl/include/fcl/traversal_node_octree.h
@@ -227,11 +227,10 @@ public:
AABB bv2;
computeBV<AABB>(s, SimpleTransform(), bv2);
- Box box2;
- SimpleTransform box2_tf;
- constructBox(bv2, tf2, box2, box2_tf);
+ OBB obb2;
+ convertBV(bv2, tf2, obb2);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
- s, box2, box2_tf,
+ s, obb2,
tf1, tf2, pairs);
}
@@ -248,11 +247,10 @@ public:
AABB bv1;
computeBV<AABB>(s, SimpleTransform(), bv1);
- Box box1;
- SimpleTransform box1_tf;
- constructBox(bv1, tf1, box1, box1_tf);
+ OBB obb1;
+ convertBV(bv1, tf1, obb1);
OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(),
- s, box1, box1_tf,
+ s, obb1,
tf2, tf1, pairs);
}
@@ -260,16 +258,12 @@ public:
FCL_REAL OcTreeShapeDistance(const OcTree* tree, const S& s,
const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
- AABB bv2;
- computeBV<AABB>(s, SimpleTransform(), bv2);
- Box box2;
- SimpleTransform box2_tf;
- constructBox(bv2, tf2, box2, box2_tf);
-
+ AABB aabb2;
+ computeBV<AABB>(s, tf2, aabb2);
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
- s, box2, box2_tf,
+ s, aabb2,
tf1, tf2, min_dist);
return min_dist;
}
@@ -278,16 +272,12 @@ public:
FCL_REAL ShapeOcTreeDistance(const S& s, const OcTree* tree,
const SimpleTransform& tf1, const SimpleTransform& tf2) const
{
- AABB bv1;
- computeBV<AABB>(s, SimpleTransform(), bv1);
- Box box1;
- SimpleTransform box1_tf;
- constructBox(bv1, tf1, box1, box1_tf);
-
+ AABB aabb1;
+ computeBV<AABB>(s, tf1, aabb1);
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(),
- s, box1, box1_tf,
+ s, aabb1,
tf2, tf1, min_dist);
return min_dist;
@@ -297,7 +287,7 @@ public:
private:
template<typename S>
bool OcTreeShapeDistanceRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
- const S& s, const Box& box2, const SimpleTransform& box2_tf,
+ const S& s, const AABB& aabb2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
FCL_REAL& min_dist) const
{
@@ -308,7 +298,7 @@ private:
Box box;
SimpleTransform box_tf;
constructBox(bv1, tf1, box, box_tf);
-
+
FCL_REAL dist;
solver->shapeDistance(box, box_tf, s, tf2, &dist);
if(dist < min_dist) min_dist = dist;
@@ -328,15 +318,13 @@ private:
const OcTree::OcTreeNode* child = root1->getChild(i);
AABB child_bv;
computeChildBV(bv1, i, child_bv);
-
- Box box1;
- SimpleTransform box1_tf;
- constructBox(child_bv, tf1, box1, box1_tf);
- FCL_REAL d;
- solver->shapeDistance(box1, box1_tf, box2, box2_tf, &d);
+
+ AABB aabb1;
+ convertBV(child_bv, tf1, aabb1);
+ FCL_REAL d = aabb1.distance(aabb2);
if(d < min_dist)
{
- if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, box2, box2_tf, tf1, tf2, min_dist))
+ if(OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1, tf2, min_dist))
return true;
}
}
@@ -347,7 +335,7 @@ private:
template<typename S>
bool OcTreeShapeIntersectRecurse(const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
- const S& s, const Box& box2, const SimpleTransform& box2_tf,
+ const S& s, const OBB& obb2,
const SimpleTransform& tf1, const SimpleTransform& tf2,
std::vector<OcTreeShapeCollisionPair>& pairs) const
{
@@ -355,38 +343,40 @@ private:
{
if(tree1->isNodeOccupied(root1))
{
- Box box;
- SimpleTransform box_tf;
- constructBox(bv1, tf1, box, box_tf);
-
- if(!enable_contact)
- {
- if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
- pairs.push_back(OcTreeShapeCollisionPair(root1));
- }
- else
+ OBB obb1;
+ convertBV(bv1, tf1, obb1);
+ if(obb1.overlap(obb2))
{
- Vec3f contact;
- FCL_REAL depth;
- Vec3f normal;
+ Box box;
+ SimpleTransform box_tf;
+ constructBox(bv1, tf1, box, box_tf);
- if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal))
- pairs.push_back(OcTreeShapeCollisionPair(root1, contact, normal, depth));
- }
+ if(!enable_contact)
+ {
+ if(solver->shapeIntersect(box, box_tf, s, tf2, NULL, NULL, NULL))
+ pairs.push_back(OcTreeShapeCollisionPair(root1));
+ }
+ else
+ {
+ Vec3f contact;
+ FCL_REAL depth;
+ Vec3f normal;
+
+ if(solver->shapeIntersect(box, box_tf, s, tf2, &contact, &depth, &normal))
+ pairs.push_back(OcTreeShapeCollisionPair(root1, contact, normal, depth));
+ }
- return ((pairs.size() >= num_max_contacts) && !exhaustive);
+ return ((pairs.size() >= num_max_contacts) && !exhaustive);
+ }
+ else return false;
}
else
return false;
}
-
- Box box1;
- SimpleTransform box1_tf;
-
- constructBox(bv1, tf1, box1, box1_tf);
-
- if(!tree1->isNodeOccupied(root1) || !solver->shapeIntersect(box1, box1_tf, box2, box2_tf, NULL, NULL, NULL)) return false;
+ OBB obb1;
+ convertBV(bv1, tf1, obb1);
+ if(!tree1->isNodeOccupied(root1) || !obb1.overlap(obb2)) return false;
for(unsigned int i = 0; i < 8; ++i)
{
@@ -396,7 +386,7 @@ private:
AABB child_bv;
computeChildBV(bv1, i, child_bv);
- if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, box2, box2_tf, tf1, tf2, pairs))
+ if(OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1, tf2, pairs))
return true;
}
}
@@ -444,14 +434,13 @@ private:
AABB child_bv;
computeChildBV(bv1, i, child_bv);
- Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
- constructBox(child_bv, tf1, box1, box1_tf);
- constructBox(tree2->getBV(root2).bv, tf2, box2, box2_tf);
+ FCL_REAL d;
+ AABB aabb1, aabb2;
+ convertBV(child_bv, tf1, aabb1);
+ convertBV(tree2->getBV(root2).bv, tf2, aabb2);
+ d = aabb1.distance(aabb2);
- FCL_REAL dist;
- solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist);
- if(dist < min_dist)
+ if(d < min_dist)
{
if(OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2, tf1, tf2, min_dist))
return true;
@@ -461,25 +450,24 @@ private:
}
else
{
- Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
- constructBox(bv1, tf1, box1, box1_tf);
-
- FCL_REAL dist;
-
+ FCL_REAL d;
+ AABB aabb1, aabb2;
+ convertBV(bv1, tf1, aabb1);
int child = tree2->getBV(root2).leftChild();
- constructBox(tree2->getBV(child).bv, tf2, box2, box2_tf);
- solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist);
- if(dist < min_dist)
+ convertBV(tree2->getBV(child).bv, tf2, aabb2);
+ d = aabb1.distance(aabb2);
+
+ if(d < min_dist)
{
if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2, min_dist))
return true;
}
child = tree2->getBV(root2).rightChild();
- constructBox(tree2->getBV(child).bv, tf2, box2, box2_tf);
- solver->shapeDistance(box1, box1_tf, box2, box2_tf, &dist);
- if(dist < min_dist)
+ convertBV(tree2->getBV(child).bv, tf2, aabb2);
+ d = aabb1.distance(aabb2);
+
+ if(d < min_dist)
{
if(OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1, tf2, min_dist))
return true;
@@ -500,45 +488,50 @@ private:
{
if(tree1->isNodeOccupied(root1))
{
- Box box;
- SimpleTransform box_tf;
- constructBox(bv1, tf1, box, box_tf);
-
- int primitive_id = tree2->getBV(root2).primitiveId();
- const Triangle& tri_id = tree2->tri_indices[primitive_id];
- const Vec3f& p1 = tree2->vertices[tri_id[0]];
- const Vec3f& p2 = tree2->vertices[tri_id[1]];
- const Vec3f& p3 = tree2->vertices[tri_id[2]];
+ OBB obb1, obb2;
+ convertBV(bv1, tf1, obb1);
+ convertBV(tree2->getBV(root2).bv, tf2, obb2);
+ if(obb1.overlap(obb2))
+ {
+ Box box;
+ SimpleTransform box_tf;
+ constructBox(bv1, tf1, box, box_tf);
+
+ int primitive_id = tree2->getBV(root2).primitiveId();
+ const Triangle& tri_id = tree2->tri_indices[primitive_id];
+ const Vec3f& p1 = tree2->vertices[tri_id[0]];
+ const Vec3f& p2 = tree2->vertices[tri_id[1]];
+ const Vec3f& p3 = tree2->vertices[tri_id[2]];
+ if(!enable_contact)
+ {
+ if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), NULL, NULL, NULL))
+ pairs.push_back(OcTreeMeshCollisionPair(root1, root2));
+ }
+ else
+ {
+ Vec3f contact;
+ FCL_REAL depth;
+ Vec3f normal;
- if(!enable_contact)
- {
- if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), NULL, NULL, NULL))
- pairs.push_back(OcTreeMeshCollisionPair(root1, root2));
- }
- else
- {
- Vec3f contact;
- FCL_REAL depth;
- Vec3f normal;
+ if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), &contact, &depth, &normal))
+ pairs.push_back(OcTreeMeshCollisionPair(root1, root2, contact, normal, depth));
+ }
- if(solver->shapeTriangleIntersect(box, box_tf, p1, p2, p3, tf2.getRotation(), tf2.getTranslation(), &contact, &depth, &normal))
- pairs.push_back(OcTreeMeshCollisionPair(root1, root2, contact, normal, depth));
+ return ((pairs.size() >= num_max_contacts) && !exhaustive);
}
-
- return ((pairs.size() >= num_max_contacts) && !exhaustive);
+ else
+ return false;
}
else
return false;
}
- Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
-
- constructBox(bv1, tf1, box1, box1_tf);
- constructBox(tree2->getBV(root2).bv, tf2, box2, box2_tf);
- if(!tree1->isNodeOccupied(root1) || !solver->shapeIntersect(box1, box1_tf, box2, box2_tf, NULL, NULL, NULL)) return false;
+ OBB obb1, obb2;
+ convertBV(bv1, tf1, obb1);
+ convertBV(tree2->getBV(root2).bv, tf2, obb2);
+ if(!tree1->isNodeOccupied(root1) || !obb1.overlap(obb2)) return false;
if(tree2->getBV(root2).isLeaf() || (root1->hasChildren() && (bv1.size() > tree2->getBV(root2).bv.size())))
{
@@ -605,11 +598,11 @@ private:
computeChildBV(bv1, i, child_bv);
FCL_REAL d;
- Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
- constructBox(child_bv, tf1, box1, box1_tf);
- constructBox(bv2, tf2, box2, box2_tf);
- solver->shapeDistance(box1, box2_tf, box2, box2_tf, &d);
+ AABB aabb1, aabb2;
+ convertBV(bv1, tf1, aabb1);
+ convertBV(bv2, tf2, aabb2);
+ d = aabb1.distance(aabb2);
+
if(d < min_dist)
{
@@ -630,11 +623,10 @@ private:
computeChildBV(bv2, i, child_bv);
FCL_REAL d;
- Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
- constructBox(bv1, tf1, box1, box1_tf);
- constructBox(child_bv, tf2, box2, box2_tf);
- solver->shapeDistance(box1, box2_tf, box2, box2_tf, &d);
+ AABB aabb1, aabb2;
+ convertBV(bv1, tf1, aabb1);
+ convertBV(bv2, tf2, aabb2);
+ d = aabb1.distance(aabb2);
if(d < min_dist)
{
@@ -658,18 +650,22 @@ private:
{
if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2))
{
- Box box1, box2;
- SimpleTransform box1_tf, box2_tf;
- constructBox(bv1, tf1, box1, box1_tf);
- constructBox(bv2, tf2, box2, box2_tf);
-
if(!enable_contact)
{
- if(solver->shapeIntersect(box1, box1_tf, box2, box2_tf, NULL, NULL, NULL))
+ OBB obb1, obb2;
+ convertBV(bv1, tf1, obb1);
+ convertBV(bv2, tf2, obb2);
+
+ if(obb1.overlap(obb2))
pairs.push_back(OcTreeCollisionPair(root1, root2));
}
else
{
+ Box box1, box2;
+ SimpleTransform box1_tf, box2_tf;
+ constructBox(bv1, tf1, box1, box1_tf);
+ constructBox(bv2, tf2, box2, box2_tf);
+
Vec3f contact;
FCL_REAL depth;
Vec3f normal;
@@ -685,6 +681,11 @@ private:
if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false;
+ OBB obb1, obb2;
+ convertBV(bv1, tf1, obb1);
+ convertBV(bv2, tf2, obb2);
+ if(!obb1.overlap(obb2)) return false;
+
if(!root2->hasChildren() || (root1->hasChildren() && (bv1.size() > bv2.size())))
{
for(unsigned int i = 0; i < 8; ++i)
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 45699ea632ceca3e11315ddc99b82bacb51ab3d7..1d01ea4dcab5c9b226b6ed0f38417711006c8e58 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -2343,7 +2343,8 @@ bool DynamicAABBTreeCollisionManager::selfDistanceRecurse(DynamicAABBNode* root,
return false;
}
-bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const
+
+bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback)
{
if(root1->isLeaf() && !root2->hasChildren())
{
@@ -2374,9 +2375,9 @@ bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, c
if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
{
- if(collisionRecurse(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ if(collisionRecurse_(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback))
return true;
- if(collisionRecurse(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback))
+ if(collisionRecurse_(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback))
return true;
}
else
@@ -2389,7 +2390,7 @@ bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, c
AABB child_bv;
computeChildBV(root2_bv, i, child_bv);
- if(collisionRecurse(root1, tree2, child, child_bv, tf2, cdata, callback))
+ if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
return true;
}
}
@@ -2397,7 +2398,144 @@ bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, c
return false;
}
-bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ const AABB& root2_bv_t = translate(root2_bv, tf2);
+ if(root1->bv.overlap(root2_bv_t))
+ {
+ Box* box = new Box();
+ SimpleTransform box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
+ }
+ else return false;
+ }
+ else return false;
+ }
+
+ const AABB& root2_bv_t = translate(root2_bv, tf2);
+ if(!tree2->isNodeOccupied(root2) || !root1->bv.overlap(root2_bv_t)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionRecurse_(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ }
+ else
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+
+ if(collisionRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback))
+ return true;
+ }
+ }
+ }
+ return false;
+}
+
+
+
+bool DynamicAABBTreeCollisionManager::collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const
+{
+ if(tf2.getQuatRotation().isIdentity())
+ return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
+ else // has rotation
+ return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback);
+}
+
+
+bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ Box* box = new Box();
+ SimpleTransform box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
+ }
+ else return false;
+ }
+
+ if(!tree2->isNodeOccupied(root2)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ const AABB& aabb2 = translate(root2_bv, tf2);
+ FCL_REAL d1 = aabb2.distance(root1->childs[0]->bv);
+ FCL_REAL d2 = aabb2.distance(root1->childs[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+ const AABB& aabb2 = translate(child_bv, tf2);
+
+ FCL_REAL d = root1->bv.distance(aabb2);
+
+ if(d < min_dist)
+ {
+ if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ }
+
+ return false;
+}
+
+
+bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
{
if(root1->isLeaf() && !root2->hasChildren())
{
@@ -2426,13 +2564,13 @@ bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, co
{
if(d2 < min_dist)
{
- if(distanceRecurse(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
return true;
}
if(d1 < min_dist)
{
- if(distanceRecurse(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
return true;
}
}
@@ -2440,13 +2578,13 @@ bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, co
{
if(d1 < min_dist)
{
- if(distanceRecurse(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
return true;
}
if(d2 < min_dist)
{
- if(distanceRecurse(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
return true;
}
}
@@ -2467,7 +2605,7 @@ bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, co
if(d < min_dist)
{
- if(distanceRecurse(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(root1, tree2, child, child_bv, tf2, cdata, callback, min_dist))
return true;
}
}
@@ -2477,10 +2615,13 @@ bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, co
return false;
}
-
-
-
-
+bool DynamicAABBTreeCollisionManager::distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ if(tf2.getQuatRotation().isIdentity())
+ return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
+ else
+ return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, cdata, callback, min_dist);
+}
@@ -2806,11 +2947,9 @@ bool DynamicAABBTreeCollisionManager2::selfDistanceRecurse(DynamicAABBNode* node
return false;
}
-
-
-bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const
+bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback)
{
- DynamicAABBNode* root1 = nodes1 + root1_id;
+ DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
if(root1->isLeaf() && !root2->hasChildren())
{
if(tree2->isNodeOccupied(root2))
@@ -2840,9 +2979,9 @@ bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1,
if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
{
- if(collisionRecurse(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ if(collisionRecurse_(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback))
return true;
- if(collisionRecurse(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback))
+ if(collisionRecurse_(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback))
return true;
}
else
@@ -2855,7 +2994,7 @@ bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1,
AABB child_bv;
computeChildBV(root2_bv, i, child_bv);
- if(collisionRecurse(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback))
+ if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback))
return true;
}
}
@@ -2864,9 +3003,70 @@ bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1,
return false;
}
-bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+bool collisionRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, CollisionCallBack callback)
{
- DynamicAABBNode* root1 = nodes1 + root1_id;
+ DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ const AABB& root_bv_t = translate(root2_bv, tf2);
+ if(root1->bv.overlap(root_bv_t))
+ {
+ Box* box = new Box();
+ SimpleTransform box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
+ }
+ else return false;
+ }
+ else return false;
+ }
+
+ const AABB& root_bv_t = translate(root2_bv, tf2);
+ if(!tree2->isNodeOccupied(root2) || !root1->bv.overlap(root_bv_t)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ if(collisionRecurse_(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ if(collisionRecurse_(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback))
+ return true;
+ }
+ else
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+
+ if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback))
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+
+
+bool DynamicAABBTreeCollisionManager2::collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const
+{
+ if(tf2.getQuatRotation().isIdentity())
+ return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback);
+ else
+ return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback);
+}
+
+
+bool distanceRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+{
+ DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
if(root1->isLeaf() && !root2->hasChildren())
{
if(tree2->isNodeOccupied(root2))
@@ -2894,13 +3094,13 @@ bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1,
{
if(d2 < min_dist)
{
- if(distanceRecurse(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
return true;
}
if(d1 < min_dist)
{
- if(distanceRecurse(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
return true;
}
}
@@ -2908,13 +3108,13 @@ bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1,
{
if(d1 < min_dist)
{
- if(distanceRecurse(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
return true;
}
if(d2 < min_dist)
{
- if(distanceRecurse(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
return true;
}
}
@@ -2935,7 +3135,7 @@ bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1,
if(d < min_dist)
{
- if(distanceRecurse(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback, min_dist))
+ if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback, min_dist))
return true;
}
}
@@ -2945,4 +3145,92 @@ bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1,
return false;
}
+
+bool distanceRecurse_(DynamicAABBTreeCollisionManager2::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Vec3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+{
+ DynamicAABBTreeCollisionManager2::DynamicAABBNode* root1 = nodes1 + root1_id;
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ Box* box = new Box();
+ SimpleTransform box_tf;
+ constructBox(root2_bv, tf2, *box, box_tf);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), box_tf);
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
+ }
+ else return false;
+ }
+
+ if(!tree2->isNodeOccupied(root2)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ const AABB& aabb2 = translate(root2_bv, tf2);
+
+ FCL_REAL d1 = aabb2.distance((nodes1 + root1->childs[0])->bv);
+ FCL_REAL d2 = aabb2.distance((nodes1 + root1->childs[1])->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->childs[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1->childs[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ const OcTree::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+
+ const AABB& aabb2 = translate(child_bv, tf2);
+ FCL_REAL d = root1->bv.distance(aabb2);
+
+ if(d < min_dist)
+ {
+ if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ }
+
+ return false;
+}
+
+bool DynamicAABBTreeCollisionManager2::distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
+{
+ if(tf2.getQuatRotation().isIdentity())
+ return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.getTranslation(), cdata, callback, min_dist);
+ else
+ return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback, min_dist);
+}
+
}