From 4229dd9efefe5c527f304292f32eafeb6f0d3ca7 Mon Sep 17 00:00:00 2001 From: Gautham Manoharan Date: Tue, 14 Jun 2016 21:39:12 +0200 Subject: [PATCH 1/4] octomap_1.8_compatible --- include/fcl/octree.h | 28 ++++++-- include/fcl/traversal/traversal_node_octree.h | 64 +++++++++---------- .../broadphase_dynamic_AABB_tree.cpp | 32 +++++----- .../broadphase_dynamic_AABB_tree_array.cpp | 32 +++++----- 4 files changed, 88 insertions(+), 68 deletions(-) diff --git a/include/fcl/octree.h b/include/fcl/octree.h index 4271e822c..62a90e8f0 100644 --- a/include/fcl/octree.h +++ b/include/fcl/octree.h @@ -63,10 +63,6 @@ class OcTree : public CollisionGeometry public: - /// @brief OcTreeNode must implement the following interfaces: - /// 1) childExists(i) - /// 2) getChild(i) - /// 3) hasChildren() typedef octomap::OcTreeNode OcTreeNode; /// @brief construct octree with a given resolution @@ -191,6 +187,30 @@ class OcTree : public CollisionGeometry free_threshold = d; } + /// @return ptr to child number childIdx of node + inline OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) const + { + return tree->getNodeChild(node, childIdx); + } + + /// @return const ptr to child number childIdx of node + inline const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const + { + return tree->getNodeChild(node, childIdx); + } + + /// @brief return true if the child at childIdx exists + inline bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const + { + return tree->nodeChildExists(node, childIdx); + } + + /// @brief return true if node has at least one child + inline bool nodeHasChildren(const OcTreeNode* node) const + { + return tree->nodeHasChildren(node); + } + /// @brief return object type, it is an octree OBJECT_TYPE getObjectType() const { return OT_OCTREE; } diff --git a/include/fcl/traversal/traversal_node_octree.h b/include/fcl/traversal/traversal_node_octree.h index 2fbd376b9..f15cc158c 100644 --- a/include/fcl/traversal/traversal_node_octree.h +++ b/include/fcl/traversal/traversal_node_octree.h @@ -239,7 +239,7 @@ class OcTreeSolver const S& s, const AABB& aabb2, const Transform3f& tf1, const Transform3f& tf2) const { - if(!root1->hasChildren()) + if(!tree1->nodeHasChildren(root1)) { if(tree1->isNodeOccupied(root1)) { @@ -263,9 +263,9 @@ class OcTreeSolver for(unsigned int i = 0; i < 8; ++i) { - if(root1->childExists(i)) + if(tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = root1->getChild(i); + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); @@ -311,7 +311,7 @@ class OcTreeSolver return false; } - else if(!root1->hasChildren()) + else if(!tree1->nodeHasChildren(root1)) { if(tree1->isNodeOccupied(root1) && s.isOccupied()) // occupied area { @@ -414,9 +414,9 @@ class OcTreeSolver for(unsigned int i = 0; i < 8; ++i) { - if(root1->childExists(i)) + if(tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = root1->getChild(i); + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); @@ -441,7 +441,7 @@ class OcTreeSolver const BVHModel* tree2, int root2, const Transform3f& tf1, const Transform3f& tf2) const { - if(!root1->hasChildren() && tree2->getBV(root2).isLeaf()) + if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { if(tree1->isNodeOccupied(root1)) { @@ -469,13 +469,13 @@ class OcTreeSolver if(!tree1->isNodeOccupied(root1)) return false; - if(tree2->getBV(root2).isLeaf() || (root1->hasChildren() && (bv1.size() > tree2->getBV(root2).bv.size()))) + if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) { for(unsigned int i = 0; i < 8; ++i) { - if(root1->childExists(i)) + if(tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = root1->getChild(i); + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); @@ -571,7 +571,7 @@ class OcTreeSolver return false; } } - else if(!root1->hasChildren() && tree2->getBV(root2).isLeaf()) + else if(!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) { if(tree1->isNodeOccupied(root1) && tree2->isOccupied()) { @@ -676,13 +676,13 @@ class OcTreeSolver if(!obb1.overlap(obb2)) return false; } - if(tree2->getBV(root2).isLeaf() || (root1->hasChildren() && (bv1.size() > tree2->getBV(root2).bv.size()))) + if(tree2->getBV(root2).isLeaf() || (tree1->nodeHasChildren(root1) && (bv1.size() > tree2->getBV(root2).bv.size()))) { for(unsigned int i = 0; i < 8; ++i) { - if(root1->childExists(i)) + if(tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = root1->getChild(i); + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); @@ -716,7 +716,7 @@ class OcTreeSolver const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3f& tf1, const Transform3f& tf2) const { - if(!root1->hasChildren() && !root2->hasChildren()) + if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) { @@ -739,13 +739,13 @@ class OcTreeSolver if(!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2)) return false; - if(!root2->hasChildren() || (root1->hasChildren() && (bv1.size() > bv2.size()))) + if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { for(unsigned int i = 0; i < 8; ++i) { - if(root1->childExists(i)) + if(tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = root1->getChild(i); + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); @@ -768,9 +768,9 @@ class OcTreeSolver { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); @@ -822,13 +822,13 @@ class OcTreeSolver } else if(!root1 && root2) { - if(root2->hasChildren()) + if(tree2->nodeHasChildren(root2)) { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); if(OcTreeIntersectRecurse(tree1, NULL, bv1, tree2, child, child_bv, tf1, tf2)) @@ -853,13 +853,13 @@ class OcTreeSolver } else if(root1 && !root2) { - if(root1->hasChildren()) + if(tree1->nodeHasChildren(root1)) { for(unsigned int i = 0; i < 8; ++i) { - if(root1->childExists(i)) + if(tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = root1->getChild(i); + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); if(OcTreeIntersectRecurse(tree1, child, child_bv, tree2, NULL, bv2, tf1, tf2)) @@ -882,7 +882,7 @@ class OcTreeSolver return false; } - else if(!root1->hasChildren() && !root2->hasChildren()) + else if(!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) { if(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) // occupied area { @@ -990,13 +990,13 @@ class OcTreeSolver if(!obb1.overlap(obb2)) return false; } - if(!root2->hasChildren() || (root1->hasChildren() && (bv1.size() > bv2.size()))) + if(!tree2->nodeHasChildren(root2) || (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) { for(unsigned int i = 0; i < 8; ++i) { - if(root1->childExists(i)) + if(tree1->nodeChildExists(root1, i)) { - const OcTree::OcTreeNode* child = root1->getChild(i); + const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i); AABB child_bv; computeChildBV(bv1, i, child_bv); @@ -1021,9 +1021,9 @@ class OcTreeSolver { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(bv2, i, child_bv); diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index 66bf37919..fa826b8c9 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -89,7 +89,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, return false; } - else if(root1->isLeaf() && !root2->hasChildren()) + else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); @@ -122,7 +122,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; - if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) return true; @@ -133,9 +133,9 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -188,7 +188,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, return false; } - else if(root1->isLeaf() && !root2->hasChildren()) + else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); @@ -215,7 +215,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const AABB& root2_bv_t = translate(root2_bv, tf2); if(tree2->isNodeFree(root2) || !root1->bv.overlap(root2_bv_t)) return false; - if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if(collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) return true; @@ -226,9 +226,9 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -250,7 +250,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { - if(root1->isLeaf() && !root2->hasChildren()) + if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if(tree2->isNodeOccupied(root2)) { @@ -265,7 +265,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c if(!tree2->isNodeOccupied(root2)) return false; - if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { AABB aabb2; convertBV(root2_bv, tf2, aabb2); @@ -306,9 +306,9 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -339,7 +339,7 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c 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(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if(tree2->isNodeOccupied(root2)) { @@ -354,7 +354,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c if(!tree2->isNodeOccupied(root2)) return false; - if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, tf2); FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); @@ -393,9 +393,9 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, c { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); const AABB& aabb2 = translate(child_bv, tf2); diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 4ec168486..486be8a8d 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -89,7 +89,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n return false; } - else if(root1->isLeaf() && !root2->hasChildren()) + else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if(!tree2->isNodeFree(root2) && !obj1->isFree()) @@ -121,7 +121,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false; - if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) return true; @@ -132,9 +132,9 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -190,7 +190,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n return false; } - else if(root1->isLeaf() && !root2->hasChildren()) + else if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { CollisionObject* obj1 = static_cast(root1->data); if(!tree2->isNodeFree(root2) && !obj1->isFree()) @@ -216,7 +216,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n const AABB& root_bv_t = translate(root2_bv, tf2); if(tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false; - if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback)) return true; @@ -227,9 +227,9 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -253,7 +253,7 @@ bool collisionRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* n bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; - if(root1->isLeaf() && !root2->hasChildren()) + if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if(tree2->isNodeOccupied(root2)) { @@ -268,7 +268,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* no if(!tree2->isNodeOccupied(root2)) return false; - if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { AABB aabb2; convertBV(root2_bv, tf2, aabb2); @@ -309,9 +309,9 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* no { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); @@ -335,7 +335,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* no bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::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) { DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* root1 = nodes1 + root1_id; - if(root1->isLeaf() && !root2->hasChildren()) + if(root1->isLeaf() && !tree2->nodeHasChildren(root2)) { if(tree2->isNodeOccupied(root2)) { @@ -350,7 +350,7 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* no if(!tree2->isNodeOccupied(root2)) return false; - if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) + if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) { const AABB& aabb2 = translate(root2_bv, tf2); @@ -390,9 +390,9 @@ bool distanceRecurse_(DynamicAABBTreeCollisionManager_Array::DynamicAABBNode* no { for(unsigned int i = 0; i < 8; ++i) { - if(root2->childExists(i)) + if(tree2->nodeChildExists(root2, i)) { - const OcTree::OcTreeNode* child = root2->getChild(i); + const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i); AABB child_bv; computeChildBV(root2_bv, i, child_bv); From 3ccb9856d9ad919ec5ab474bd07cf388b4517247 Mon Sep 17 00:00:00 2001 From: "Javier V. Gomez" Date: Wed, 15 Jun 2016 13:25:05 +0200 Subject: [PATCH 2/4] Update CI's Octomap version --- ci/install_linux.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ci/install_linux.sh b/ci/install_linux.sh index a6e54b3ab..831bde11e 100755 --- a/ci/install_linux.sh +++ b/ci/install_linux.sh @@ -6,7 +6,7 @@ sudo apt-get -qq --yes --force-yes install libccd-dev # Octomap git clone https://github.com/OctoMap/octomap cd octomap -git checkout tags/v1.6.8 +git checkout tags/v1.8.0 mkdir build cd build cmake .. From dd7796851fe367f4f3e4989b66366cde29cf8128 Mon Sep 17 00:00:00 2001 From: "Javier V. Gomez" Date: Wed, 15 Jun 2016 20:16:23 +0200 Subject: [PATCH 3/4] Remove unecessary inlines, const correctess, and fix indentation --- include/fcl/octree.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/include/fcl/octree.h b/include/fcl/octree.h index 62a90e8f0..8839a0bc3 100644 --- a/include/fcl/octree.h +++ b/include/fcl/octree.h @@ -94,7 +94,7 @@ class OcTree : public CollisionGeometry } /// @brief get the bounding volume for the root - inline AABB getRootBV() const + AABB getRootBV() const { FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2; @@ -103,34 +103,34 @@ class OcTree : public CollisionGeometry } /// @brief get the root node of the octree - inline OcTreeNode* getRoot() const + OcTreeNode* getRoot() const { return tree->getRoot(); } /// @brief whether one node is completely occupied - inline bool isNodeOccupied(const OcTreeNode* node) const + bool isNodeOccupied(const OcTreeNode* node) const { // return tree->isNodeOccupied(node); return node->getOccupancy() >= occupancy_threshold; } /// @brief whether one node is completely free - inline bool isNodeFree(const OcTreeNode* node) const + bool isNodeFree(const OcTreeNode* node) const { // return false; // default no definitely free node return node->getOccupancy() <= free_threshold; } /// @brief whether one node is uncertain - inline bool isNodeUncertain(const OcTreeNode* node) const + bool isNodeUncertain(const OcTreeNode* node) const { return (!isNodeOccupied(node)) && (!isNodeFree(node)); } /// @brief transform the octree into a bunch of boxes; uncertainty information is kept in the boxes. However, we /// only keep the occupied boxes (i.e., the boxes whose occupied probability is higher enough). - inline std::vector > toBoxes() const + std::vector > toBoxes() const { std::vector > boxes; boxes.reserve(tree->size() / 2); @@ -188,29 +188,29 @@ class OcTree : public CollisionGeometry } /// @return ptr to child number childIdx of node - inline OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) const + OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) { return tree->getNodeChild(node, childIdx); } /// @return const ptr to child number childIdx of node - inline const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const + const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const { return tree->getNodeChild(node, childIdx); } /// @brief return true if the child at childIdx exists - inline bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const + bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const { return tree->nodeChildExists(node, childIdx); } /// @brief return true if node has at least one child - inline bool nodeHasChildren(const OcTreeNode* node) const + bool nodeHasChildren(const OcTreeNode* node) const { - return tree->nodeHasChildren(node); + return tree->nodeHasChildren(node); } - + /// @brief return object type, it is an octree OBJECT_TYPE getObjectType() const { return OT_OCTREE; } From 3558c1e739278ef60b328aab68bb953b040ea273 Mon Sep 17 00:00:00 2001 From: "Javier V. Gomez" Date: Thu, 16 Jun 2016 17:19:38 +0200 Subject: [PATCH 4/4] Octomap <1.8.0 compatible but not working --- CMakeLists.txt | 4 ++++ include/fcl/config.h.in | 16 ++++++++++++++++ include/fcl/octree.h | 16 ++++++++++++++++ 3 files changed, 36 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3791d4a3f..3370573b7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,6 +82,10 @@ if(NOT OCTOMAP_FOUND) endif() endif() if (OCTOMAP_FOUND OR (OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS)) + string(REPLACE "." ";" VERSION_LIST ${OCTOMAP_VERSION}) + list(GET VERSION_LIST 0 OCTOMAP_MAJOR_VERSION) + list(GET VERSION_LIST 1 OCTOMAP_MINOR_VERSION) + list(GET VERSION_LIST 2 OCTOMAP_PATCH_VERSION) include_directories(${OCTOMAP_INCLUDE_DIRS}) link_directories(${OCTOMAP_LIBRARY_DIRS}) set(FCL_HAVE_OCTOMAP 1) diff --git a/include/fcl/config.h.in b/include/fcl/config.h.in index 3dfa0299f..2503c9505 100644 --- a/include/fcl/config.h.in +++ b/include/fcl/config.h.in @@ -48,4 +48,20 @@ #cmakedefine01 FCL_BUILD_TYPE_DEBUG #cmakedefine01 FCL_BUILD_TYPE_RELEASE +#if FCL_HAVE_OCTOMAP + #define OCTOMAP_MAJOR_VERSION @OCTOMAP_MAJOR_VERSION@ + #define OCTOMAP_MINOR_VERSION @OCTOMAP_MINOR_VERSION@ + #define OCTOMAP_PATCH_VERSION @OCTOMAP_PATCH_VERSION@ + + #define OCTOMAP_VERSION_AT_LEAST(x,y,z) \ + (OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \ + (OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \ + OCTOMAP_PATCH_VERSION >= z)))) + + #define OCTOMAP_VERSION_AT_MOST(x,y,z) \ + (OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \ + (OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \ + OCTOMAP_PATCH_VERSION <= z)))) +#endif // FCL_HAVE_OCTOMAP + #endif diff --git a/include/fcl/octree.h b/include/fcl/octree.h index 8839a0bc3..5c57d4d57 100644 --- a/include/fcl/octree.h +++ b/include/fcl/octree.h @@ -190,25 +190,41 @@ class OcTree : public CollisionGeometry /// @return ptr to child number childIdx of node OcTreeNode* getNodeChild(OcTreeNode* node, unsigned int childIdx) { +#if OCTOMAP_VERSION_AT_LEAST(1,8,0) return tree->getNodeChild(node, childIdx); +#else + return node->getChild(childIdx); +#endif } /// @return const ptr to child number childIdx of node const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const { +#if OCTOMAP_VERSION_AT_LEAST(1,8,0) return tree->getNodeChild(node, childIdx); +#else + return node->getChild(childIdx); +#endif } /// @brief return true if the child at childIdx exists bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const { +#if OCTOMAP_VERSION_AT_LEAST(1,8,0) return tree->nodeChildExists(node, childIdx); +#else + return node->childExists(childIdx); +#endif } /// @brief return true if node has at least one child bool nodeHasChildren(const OcTreeNode* node) const { +#if OCTOMAP_VERSION_AT_LEAST(1,8,0) return tree->nodeHasChildren(node); +#else + return node->hasChildren(); +#endif } /// @brief return object type, it is an octree