00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef OST_GFX_IMPL_MAP_OCTREE_HH
00020 #define OST_GFX_IMPL_MAP_OCTREE_HH
00021
00022 #include <ost/img/image_handle.hh>
00023 #include <ost/img/image_state.hh>
00024 #include <ost/gfx/module_config.hh>
00025 #include <ost/stdint.hh>
00026
00027
00028
00029 namespace ost { namespace gfx { namespace impl {
00030
00031 typedef uint16_t OcRange;
00032
00033 struct OcRangeVector {
00034 OcRangeVector(uint16_t px, uint16_t py, uint16_t pz):
00035 x(px), y(py), z(pz)
00036 { }
00037 OcRange x;
00038 OcRange y;
00039 OcRange z;
00040 };
00041
00046 class OctreeNode {
00047 public:
00048 OctreeNode(uint32_t first_child, bool branch_x, bool branch_y, bool branch_z):
00049 first_child_(first_child), branch_x_(branch_x),
00050 branch_y_(branch_y), branch_z_(branch_z), is_leaf_(false),
00051 min_(0.0), max_(0.0)
00052 { }
00053 OctreeNode(): first_child_(0),
00054 branch_x_(false), branch_y_(false), branch_z_(false), is_leaf_(false),
00055 min_(0.0), max_(0.0)
00056 { }
00058 bool BranchInX() const { return branch_x_; }
00060 bool BranchInY() const { return branch_y_; }
00062 bool BranchInZ() const { return branch_z_; }
00063
00065 uint8_t GetChildCount() const {
00066 uint8_t sum=(uint8_t(branch_x_)+uint8_t(branch_y_)+uint8_t(branch_z_));
00067 switch (sum) {
00068 case 0:
00069 return 0;
00070 case 1:
00071 return 2;
00072 case 2:
00073 return 4;
00074 case 3:
00075 return 8;
00076 default:
00077 assert(0 && "what the heck?");
00078 }
00079 return 0;
00080 }
00081 uint32_t GetFirstChild() const {
00082 return first_child_;
00083 }
00084 float GetMin() const { return min_; }
00085 float GetMax() const { return max_; }
00086 bool IsLeaf() const { return is_leaf_; }
00087 void SetLeaf() { is_leaf_=true; }
00088 void SetMin(float m) { min_=m; }
00089 void SetMax(float m) { max_=m; }
00090 private:
00091 uint32_t first_child_ : 28;
00092 bool branch_x_ : 1;
00093 bool branch_y_ : 1;
00094 bool branch_z_ : 1;
00095 bool is_leaf_ : 1;
00096 float min_;
00097 float max_;
00098 };
00099
00100 typedef std::vector<OctreeNode> OcNodeEntryList;
00101
00102
00109 class DLLEXPORT_OST_GFX MapOctree {
00110 public:
00111 MapOctree(const img::ImageHandle& map);
00112 void Initialize();
00113 uint32_t GetNumNodesForLevel(uint8_t level) const;
00114 void SetNewMap(const img::ImageHandle& ih);
00115 static bool IsMapManageable (const img::ImageHandle ih);
00116
00118 template <typename F>
00119 void VisitDF(F& f) const
00120 {
00121 if (levels_.empty()) {
00122 return;
00123 }
00124 img::Extent ext=map_.GetExtent();
00125 img::RealSpatialImageState* map=NULL;
00126 map=dynamic_cast<img::RealSpatialImageState*>(map_.ImageStatePtr().get());
00127 assert(levels_[0].size()==1);
00128 if (f.VisitNode(levels_[0][0], 0, ext)) {
00129 this->VisitDFRec<F>(levels_[0][0], f, 1, ext, map);
00130 }
00131 }
00132
00133 protected:
00134 static inline int LastSetBit(uint16_t ch)
00135 {
00136 for (char i=15; i>=0; --i) {
00137 uint16_t m=1<<i;
00138 if (ch & m) {
00139 return i;
00140 }
00141 }
00142 return -1;
00143 }
00144
00145 template <typename F>
00146 void VisitDFRec(const OctreeNode& node, F& f, uint8_t level,
00147 const img::Extent& ext,
00148 img::RealSpatialImageState* map) const
00149 {
00150 if (node.GetChildCount()==0) { return; }
00151 uint32_t c=node.GetFirstChild();
00152 int cx=1+int(node.BranchInX());
00153 int cy=1+int(node.BranchInY());
00154 int cz=1+int(node.BranchInZ());
00155 int range_x[2][2];
00156 int range_y[2][2];
00157 int range_z[2][2];
00158
00159 if (node.BranchInX()) {
00160 uint16_t ems=ext.GetEnd()[0]-ext.GetStart()[0];
00161 uint16_t bit_mask=1<<(LastSetBit(ems));
00162 range_x[0][0]=ext.GetStart()[0];
00163 range_x[0][1]=range_x[0][0]+bit_mask-1;
00164 range_x[1][0]=ext.GetEnd()[0]-(ems & ~bit_mask);
00165 range_x[1][1]=ext.GetEnd()[0];
00166 } else {
00167 range_x[0][0]=ext.GetStart()[0];
00168 range_x[0][1]=ext.GetEnd()[0];
00169 }
00170 if (node.BranchInY()) {
00171 uint16_t ems=ext.GetEnd()[1]-ext.GetStart()[1];
00172 uint16_t bit_mask=1<<(LastSetBit(ems));
00173 range_y[0][0]=ext.GetStart()[1];
00174 range_y[0][1]=range_y[0][0]+bit_mask-1;
00175 range_y[1][0]=ext.GetEnd()[1]-(ems & ~bit_mask);
00176 range_y[1][1]=ext.GetEnd()[1];
00177 } else {
00178 range_y[0][0]=ext.GetStart()[1];
00179 range_y[0][1]=ext.GetEnd()[1];
00180 }
00181 if (node.BranchInZ()) {
00182 uint16_t ems=ext.GetEnd()[2]-ext.GetStart()[2];
00183 uint16_t bit_mask=1<<(LastSetBit(ems));
00184 range_z[0][0]=ext.GetStart()[2];
00185 range_z[0][1]=range_z[0][0]+bit_mask-1;
00186 range_z[1][0]=ext.GetEnd()[2]-(ems & ~bit_mask);
00187 range_z[1][1]=ext.GetEnd()[2];
00188 } else {
00189 range_z[0][0]=ext.GetStart()[2];
00190 range_z[0][1]=ext.GetEnd()[2];
00191 }
00192 for (int i=0; i<cx; ++i) {
00193 for (int j=0; j<cy; ++j) {
00194 for (int k=0; k<cz; ++k, ++c) {
00195 if (node.IsLeaf()) {
00196 f.VisitLeaf(map, img::Point(range_x[i][0], range_y[j][0],
00197 range_z[k][0]));
00198 } else {
00199 const OctreeNode& cn=levels_[level][c];
00200 img::Extent ext(img::Point(range_x[i][0], range_y[j][0],
00201 range_z[k][0]),
00202 img::Point(range_x[i][1], range_y[j][1],
00203 range_z[k][1]));
00204 if (f.VisitNode(cn, level, ext)) {
00205 this->VisitDFRec<F>(cn, f, level+1, ext, map);
00206 }
00207 }
00208 }
00209 }
00210 }
00211 }
00212 private:
00213 void BuildOctree();
00214 std::pair<float, float> BuildOctreeRec(const OcRangeVector& range_vec,
00215 uint16_t level,
00216 img::RealSpatialImageState* map,
00217 const img::Extent& ext,
00218 OctreeNode& parent);
00219
00220 img::ImageHandle map_;
00221 std::vector<OcNodeEntryList> levels_;
00222 bool built_;
00223 };
00224
00225 }}}
00226
00227 #endif
