00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00038 #ifndef FCL_OCTREE_H
00039 #define FCL_OCTREE_H
00040
00041
00042 #include <boost/shared_ptr.hpp>
00043 #include <boost/array.hpp>
00044
00045 #include <octomap/octomap.h>
00046 #include "fcl/BV/AABB.h"
00047 #include "fcl/collision_object.h"
00048
00049 namespace fcl
00050 {
00051
00053 class OcTree : public CollisionGeometry
00054 {
00055 private:
00056 boost::shared_ptr<octomap::OcTree> tree;
00057 public:
00058
00063 typedef octomap::OcTreeNode OcTreeNode;
00064
00066 OcTree(FCL_REAL resolution) : tree(boost::shared_ptr<octomap::OcTree>(new octomap::OcTree(resolution))) {}
00067
00069 OcTree(const boost::shared_ptr<octomap::OcTree>& tree_) : tree(tree_) {}
00070
00072 void computeLocalAABB()
00073 {
00074 aabb_local = getRootBV();
00075 aabb_center = aabb_local.center();
00076 aabb_radius = (aabb_local.min_ - aabb_center).length();
00077 }
00078
00080 inline AABB getRootBV() const
00081 {
00082 FCL_REAL delta = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
00083 return AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta));
00084 }
00085
00087 inline OcTreeNode* getRoot() const
00088 {
00089 return tree->getRoot();
00090 }
00091
00093 inline bool isNodeOccupied(const OcTreeNode* node) const
00094 {
00095 return tree->isNodeOccupied(node);
00096 }
00097
00099 inline bool isNodeFree(const OcTreeNode* node) const
00100 {
00101 return false;
00102 }
00103
00105 inline bool isNodeUncertain(const OcTreeNode* node) const
00106 {
00107 return (!isNodeOccupied(node)) && (!isNodeFree(node));
00108 }
00109
00111 inline void updateNode(FCL_REAL x, FCL_REAL y, FCL_REAL z, bool occupied)
00112 {
00113 tree->updateNode(octomap::point3d(x, y, z), occupied);
00114 }
00115
00118 inline std::vector<boost::array<FCL_REAL, 6> > toBoxes() const
00119 {
00120 std::vector<boost::array<FCL_REAL, 6> > boxes;
00121 boxes.reserve(tree->size() / 2);
00122 for(octomap::OcTree::iterator it = tree->begin(tree->getTreeDepth()), end = tree->end();
00123 it != end;
00124 ++it)
00125 {
00126 if(tree->isNodeOccupied(*it))
00127 {
00128 FCL_REAL size = it.getSize();
00129 FCL_REAL x = it.getX();
00130 FCL_REAL y = it.getY();
00131 FCL_REAL z = it.getZ();
00132 FCL_REAL c = (*it).getOccupancy();
00133 FCL_REAL t = tree->getOccupancyThres();
00134
00135 boost::array<FCL_REAL, 6> box = {{x, y, z, size, c, t}};
00136 boxes.push_back(box);
00137 }
00138 }
00139 return boxes;
00140 }
00141
00143 FCL_REAL getOccupancyThres() const
00144 {
00145 return tree->getOccupancyThres();
00146 }
00147
00149 OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
00150
00152 NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
00153 };
00154
00156 static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv)
00157 {
00158 if(i&1)
00159 {
00160 child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
00161 child_bv.max_[0] = root_bv.max_[0];
00162 }
00163 else
00164 {
00165 child_bv.min_[0] = root_bv.min_[0];
00166 child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
00167 }
00168
00169 if(i&2)
00170 {
00171 child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
00172 child_bv.max_[1] = root_bv.max_[1];
00173 }
00174 else
00175 {
00176 child_bv.min_[1] = root_bv.min_[1];
00177 child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
00178 }
00179
00180 if(i&4)
00181 {
00182 child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
00183 child_bv.max_[2] = root_bv.max_[2];
00184 }
00185 else
00186 {
00187 child_bv.min_[2] = root_bv.min_[2];
00188 child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
00189 }
00190 }
00191
00192
00193
00194 }
00195
00196 #endif