Skip to content
Snippets Groups Projects
Commit 637e1556 authored by jpan's avatar jpan
Browse files

wrap up of octomap 

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@131 253336fb-580f-4252-a368-f3cef5a2a82b
parent 63022542
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
trunk/fcl/include/fcl/octomap_extension.h
+ 7
− 7
View file @ 637e1556
......@@ -40,7 +40,7 @@
#include <vector>
#include <set>
#include <octomap/octomap.h>
#include "fcl/octree.h"
#include "fcl/broad_phase_collision.h"
namespace fcl
......@@ -48,10 +48,10 @@ namespace fcl
struct OcTreeNode_AABB_pair
{
octomap::OcTreeNode* node;
OcTree::OcTreeNode* node;
AABB aabb;
OcTreeNode_AABB_pair(octomap::OcTreeNode* node_, const AABB& aabb_) : node(node_), aabb(aabb_) {}
OcTreeNode_AABB_pair(OcTree::OcTreeNode* node_, const AABB& aabb_) : node(node_), aabb(aabb_) {}
bool operator < (const OcTreeNode_AABB_pair& other) const
{
......@@ -69,13 +69,13 @@ typedef bool (*CollisionCostOctomapCallBack)(CollisionObject* o1, CollisionObjec
typedef bool (*CollisionCostOctomapCallBackExt)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes);
void collide(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCallBack callback);
void collide(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCallBack callback);
void distance(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, DistanceCallBack callback);
void distance(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, DistanceCallBack callback);
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBack callback);
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCostOctomapCallBack callback);
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBackExt callback, std::vector<AABB>& nodes);
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCostOctomapCallBackExt callback, std::vector<AABB>& nodes);
}
......
trunk/fcl/src/octomap_extension.cpp
+ 35
− 24
View file @ 637e1556
......@@ -86,11 +86,11 @@ public:
}
FCL_REAL prob;
octomap::OcTreeNode* node;
OcTree::OcTreeNode* node;
};
bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& root2_bv,
void* cdata, CollisionCallBack callback)
{
if(root1->isLeaf() && !root2->hasChildren())
......@@ -125,7 +125,7 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
if(root2->childExists(i))
{
octomap::OcTreeNode* child = root2->getChild(i);
OcTree::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
computeChildBV(root2_bv, i, child_bv);
......@@ -139,20 +139,18 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
}
void collide(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCallBack callback)
void collide(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCallBack callback)
{
DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
octomap::OcTreeNode* root2 = octree->getRoot();
FCL_REAL delta = (1 << octree->getTreeDepth()) * octree->getResolution() / 2;
OcTree::OcTreeNode* root2 = octree->getRoot();
collisionRecurse(root1, octree, root2, AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)),
collisionRecurse(root1, octree, root2, octree->getRootBV(),
cdata, callback);
}
bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& root2_bv,
void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
{
if(root1->isLeaf() && !root2->hasChildren())
......@@ -210,12 +208,17 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
if(root2->childExists(i))
{
octomap::OcTreeNode* child = root2->getChild(i);
OcTree::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
computeChildBV(root2_bv, i, child_bv);
if(distanceRecurse(root1, tree2, child, child_bv, cdata, callback, min_dist))
return true;
FCL_REAL d = root1->bv.distance(child_bv);
if(d < min_dist)
{
if(distanceRecurse(root1, tree2, child, child_bv, cdata, callback, min_dist))
return true;
}
}
}
}
......@@ -224,8 +227,18 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
}
void distance(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, DistanceCallBack callback)
{
DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
OcTree::OcTreeNode* root2 = octree->getRoot();
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(root1, octree, root2, octree->getRootBV(),
cdata, callback, min_dist);
}
bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& root2_bv,
void* cdata, CollisionCostOctomapCallBack callback, FCL_REAL& cost)
{
if(root1->isLeaf() && !root2->hasChildren())
......@@ -261,7 +274,7 @@ bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root
{
if(root2->childExists(i))
{
octomap::OcTreeNode* child = root2->getChild(i);
OcTree::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
computeChildBV(root2_bv, i, child_bv);
......@@ -276,7 +289,7 @@ bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root
bool collisionCostExtRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
OcTree* tree2, OcTree::OcTreeNode* root2, const AABB& root2_bv,
void* cdata, CollisionCostOctomapCallBackExt callback, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes)
{
if(root1->isLeaf() && !root2->hasChildren())
......@@ -311,7 +324,7 @@ bool collisionCostExtRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* r
{
if(root2->childExists(i))
{
octomap::OcTreeNode* child = root2->getChild(i);
OcTree::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
computeChildBV(root2_bv, i, child_bv);
......@@ -352,26 +365,24 @@ bool defaultCollisionCostOctomapExtFunction(CollisionObject* o1, CollisionObject
return false;
}
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBack callback)
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCostOctomapCallBack callback)
{
DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
octomap::OcTreeNode* root2 = octree->getRoot();
OcTree::OcTreeNode* root2 = octree->getRoot();
FCL_REAL delta = (1 << octree->getTreeDepth()) * octree->getResolution() / 2;
FCL_REAL cost = 0;
collisionCostRecurse(root1, octree, root2, AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)), cdata, callback, cost);
collisionCostRecurse(root1, octree, root2, octree->getRootBV(), cdata, callback, cost);
return cost;
}
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBackExt callback, std::vector<AABB>& nodes)
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, OcTree* octree, void* cdata, CollisionCostOctomapCallBackExt callback, std::vector<AABB>& nodes)
{
DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
octomap::OcTreeNode* root2 = octree->getRoot();
OcTree::OcTreeNode* root2 = octree->getRoot();
FCL_REAL delta = (1 << octree->getTreeDepth()) * octree->getResolution() / 2;
FCL_REAL cost = 0;
std::set<OcTreeNode_AABB_pair> pairs;
collisionCostExtRecurse(root1, octree, root2, AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)), cdata, callback, cost, pairs);
collisionCostExtRecurse(root1, octree, root2, octree->getRootBV(), cdata, callback, cost, pairs);
for(std::set<OcTreeNode_AABB_pair>::iterator it = pairs.begin(), end = pairs.end(); it != end; ++it)
nodes.push_back(it->aabb);
return cost;
......
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