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 #ifndef opengl_COctreePointRenderer_H
00029 #define opengl_COctreePointRenderer_H
00030
00031 #include <mrpt/opengl/CRenderizable.h>
00032 #include <mrpt/opengl/CSetOfObjects.h>
00033 #include <mrpt/opengl/CBox.h>
00034
00035
00036 namespace mrpt
00037 {
00038 namespace global_settings
00039 {
00040
00041
00042
00043
00044 extern OPENGL_IMPEXP float OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL;
00045
00046
00047
00048
00049
00050 extern OPENGL_IMPEXP size_t OCTREE_RENDER_MAX_POINTS_PER_NODE;
00051 }
00052
00053
00054 namespace opengl
00055 {
00056 using namespace mrpt::utils;
00057
00058
00059
00060
00061 template <class Derived>
00062 class COctreePointRenderer
00063 {
00064 public:
00065
00066 COctreePointRenderer() :
00067 m_octree_has_to_rebuild_all(true),
00068 m_visible_octree_nodes(0),
00069 m_visible_octree_nodes_ongoing(0)
00070 { }
00071
00072
00073 COctreePointRenderer(const COctreePointRenderer &) :
00074 m_octree_has_to_rebuild_all(true)
00075 { }
00076
00077
00078 enum { OCTREE_ROOT_NODE = 0 };
00079
00080 protected:
00081
00082 inline Derived & octree_derived() { return *static_cast<Derived*>(this); }
00083 inline const Derived & octree_derived() const { return *static_cast<const Derived*>(this); }
00084
00085
00086 inline void octree_assure_uptodate() const
00087 {
00088 const_cast<COctreePointRenderer<Derived>*>(this)->internal_octree_assure_uptodate();
00089 }
00090
00091
00092
00093
00094 void octree_render(const mrpt::opengl::CRenderizable::TRenderInfo &ri ) const
00095 {
00096 m_visible_octree_nodes_ongoing = 0;
00097
00098
00099 m_render_queue.clear();
00100 m_render_queue.reserve(m_octree_nodes.size());
00101
00102 TPixelCoordf cr_px[8];
00103 float cr_z[8];
00104 octree_recursive_render(OCTREE_ROOT_NODE,ri, cr_px, cr_z, false );
00105
00106 m_visible_octree_nodes = m_visible_octree_nodes_ongoing;
00107
00108
00109 for (size_t i=0;i<m_render_queue.size();i++)
00110 {
00111 const TNode & node = m_octree_nodes[ m_render_queue[i].node_id ];
00112 octree_derived().render_subset( node.all,node.pts,m_render_queue[i].render_area_sqpixels);
00113 }
00114 }
00115
00116
00117 private:
00118
00119
00120
00121 struct OPENGL_IMPEXP TNode
00122 {
00123 TNode() :
00124 bb_min( std::numeric_limits<float>::max(), std::numeric_limits<float>::max(), std::numeric_limits<float>::max() ),
00125 bb_max(-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max() )
00126 { }
00127
00128 bool is_leaf;
00129
00130
00131 mrpt::math::TPoint3Df bb_min, bb_max;
00132
00133
00134 std::vector<size_t> pts;
00135 bool all;
00136
00137
00138 mrpt::math::TPoint3Df center;
00139 size_t child_id[8];
00140
00141
00142 inline void update_bb(const mrpt::math::TPoint3Df &p)
00143 {
00144 keep_min(bb_min.x, p.x); keep_min(bb_min.y, p.y); keep_min(bb_min.z, p.z);
00145 keep_max(bb_max.x, p.x); keep_max(bb_max.y, p.y); keep_max(bb_max.z, p.z);
00146 }
00147
00148 inline float getCornerX(int i) const { return (i & 0x01)==0 ? bb_min.x : bb_max.x; }
00149 inline float getCornerY(int i) const { return (i & 0x02)==0 ? bb_min.y : bb_max.y; }
00150 inline float getCornerZ(int i) const { return (i & 0x04)==0 ? bb_min.z : bb_max.z; }
00151
00152 void setBBFromOrderInParent(const TNode &parent, int my_child_index)
00153 {
00154
00155 switch (my_child_index)
00156 {
00157 case 0:
00158 bb_min = parent.bb_min;
00159 bb_max = parent.center;
00160 break;
00161 case 1:
00162 bb_min.x = parent.center.x; bb_max.x = parent.bb_max.x;
00163 bb_min.y = parent.bb_min.y; bb_max.y = parent.center.y;
00164 bb_min.z = parent.bb_min.z; bb_max.z = parent.center.z;
00165 break;
00166 case 2:
00167 bb_min.x = parent.bb_min.x; bb_max.x = parent.center.x;
00168 bb_min.y = parent.center.y; bb_max.y = parent.bb_max.y;
00169 bb_min.z = parent.bb_min.z; bb_max.z = parent.center.z;
00170 break;
00171 case 3:
00172 bb_min.x = parent.center.x; bb_max.x = parent.bb_max.x;
00173 bb_min.y = parent.center.y; bb_max.y = parent.bb_max.y;
00174 bb_min.z = parent.bb_min.z; bb_max.z = parent.center.z;
00175 break;
00176 case 4:
00177 bb_min.x = parent.bb_min.x; bb_max.x = parent.center.x;
00178 bb_min.y = parent.bb_min.y; bb_max.y = parent.center.y;
00179 bb_min.z = parent.center.z; bb_max.z = parent.bb_max.z;
00180 break;
00181 case 5:
00182 bb_min.x = parent.center.x; bb_max.x = parent.bb_max.x;
00183 bb_min.y = parent.bb_min.y; bb_max.y = parent.center.y;
00184 bb_min.z = parent.center.z; bb_max.z = parent.bb_max.z;
00185 break;
00186 case 6:
00187 bb_min.x = parent.bb_min.x; bb_max.x = parent.center.x;
00188 bb_min.y = parent.center.y; bb_max.y = parent.bb_max.y;
00189 bb_min.z = parent.center.z; bb_max.z = parent.bb_max.z;
00190 break;
00191 case 7:
00192 bb_min = parent.center;
00193 bb_max = parent.bb_max;
00194 break;
00195 default: throw std::runtime_error("my_child_index!=[0,7]");
00196 }
00197 }
00198 };
00199
00200 struct OPENGL_IMPEXP TRenderQueueElement
00201 {
00202 inline TRenderQueueElement(const size_t id, float area_sq) : node_id(id), render_area_sqpixels(area_sq) { }
00203
00204 size_t node_id;
00205 float render_area_sqpixels;
00206 };
00207 mutable std::vector<TRenderQueueElement> m_render_queue;
00208
00209
00210 bool m_octree_has_to_rebuild_all;
00211 std::deque<TNode> m_octree_nodes;
00212
00213
00214 volatile mutable size_t m_visible_octree_nodes, m_visible_octree_nodes_ongoing;
00215
00216
00217 void octree_recursive_render(
00218 size_t node_idx,
00219 const mrpt::opengl::CRenderizable::TRenderInfo &ri,
00220 TPixelCoordf cr_px[8],
00221 float cr_z[8],
00222 bool corners_are_all_computed = true,
00223 bool trust_me_youre_visible = false,
00224 float approx_area_sqpixels = 0
00225 ) const
00226 {
00227 const TNode &node = m_octree_nodes[node_idx];
00228
00229 if (!corners_are_all_computed)
00230 {
00231 for (int i=0;i<8;i++)
00232 {
00233
00234 ri.projectPointPixels(
00235 node.getCornerX(i),node.getCornerY(i),node.getCornerZ(i),
00236 cr_px[i].x,cr_px[i].y,cr_z[i]);
00237 }
00238 }
00239
00240 TPixelCoordf px_min( std::numeric_limits<float>::max(),std::numeric_limits<float>::max()), px_max(-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max());
00241 if (!trust_me_youre_visible)
00242 {
00243
00244 for (int i=0;i<8;i++)
00245 {
00246 keep_min(px_min.x,cr_px[i].x); keep_min(px_min.y,cr_px[i].y);
00247 keep_max(px_max.x,cr_px[i].x); keep_max(px_max.y,cr_px[i].y);
00248 }
00249
00250 const bool any_cr_zs_neg = (cr_z[0]<0 ||cr_z[1]<0 ||cr_z[2]<0 ||cr_z[3]<0 ||cr_z[4]<0 ||cr_z[5]<0 ||cr_z[6]<0 ||cr_z[7]<0);
00251 const bool any_cr_zs_pos = (cr_z[0]>0 ||cr_z[1]>0 ||cr_z[2]>0 ||cr_z[3]>0 ||cr_z[4]>0 ||cr_z[5]>0 ||cr_z[6]>0 ||cr_z[7]>0);
00252 const bool box_crosses_image_plane = any_cr_zs_pos && any_cr_zs_neg;
00253
00254
00255
00256 if (!box_crosses_image_plane && ( px_min.x>=ri.vp_width || px_min.y>=ri.vp_height || px_max.x<0 || px_max.y<0) )
00257 return;
00258 }
00259
00260
00261 if (node.is_leaf)
00262 {
00263 if (node.all || !node.pts.empty())
00264 {
00265
00266 m_visible_octree_nodes_ongoing++;
00267
00268 const float render_area_sqpixels = trust_me_youre_visible ?
00269 approx_area_sqpixels
00270 :
00271 std::abs(px_min.x-px_max.x) * std::abs(px_min.y-px_max.y);
00272
00273
00274 m_render_queue.push_back( TRenderQueueElement(node_idx,render_area_sqpixels) );
00275 }
00276 }
00277 else
00278 {
00279
00280
00281 bool children_are_all_visible_for_sure = true;
00282
00283 if (!trust_me_youre_visible)
00284 {
00285 for (int i=0;i<8;i++)
00286 {
00287 if (!( cr_px[i].x>=0 && cr_px[i].y>=0 && cr_px[i].x<ri.vp_width && cr_px[i].y<ri.vp_height ))
00288 {
00289 children_are_all_visible_for_sure = false;
00290 break;
00291 }
00292 }
00293 }
00294
00295
00296 if (children_are_all_visible_for_sure)
00297 {
00298 TPixelCoordf child_cr_px[8];
00299 float child_cr_z[8];
00300
00301
00302 const float approx_child_area = trust_me_youre_visible ?
00303 approx_area_sqpixels/8.0f
00304 :
00305 std::abs(px_min.x-px_max.x) * std::abs(px_min.y-px_max.y) / 8.0f;
00306
00307 for (int i=0;i<8;i++)
00308 this->octree_recursive_render(node.child_id[i],ri,child_cr_px, child_cr_z, true, true, approx_child_area); \
00309 }
00310 else
00311 {
00312
00313 const TPoint3Df p_Xm_Ym_Zm ( node.bb_min.x, node.bb_min.y, node.bb_min.z );
00314 const TPoint3Df p_X0_Ym_Zm ( node.center.x, node.bb_min.y, node.bb_min.z );
00315 const TPoint3Df p_Xp_Ym_Zm ( node.bb_max.x, node.bb_min.y, node.bb_min.z );
00316 const TPoint3Df p_Xm_Y0_Zm ( node.bb_min.x, node.center.y, node.bb_min.z );
00317 const TPoint3Df p_X0_Y0_Zm ( node.center.x, node.center.y, node.bb_min.z );
00318 const TPoint3Df p_Xp_Y0_Zm ( node.bb_max.x, node.center.y, node.bb_min.z );
00319 const TPoint3Df p_Xm_Yp_Zm ( node.bb_min.x, node.bb_max.y, node.bb_min.z );
00320 const TPoint3Df p_X0_Yp_Zm ( node.center.x, node.bb_max.y, node.bb_min.z );
00321 const TPoint3Df p_Xp_Yp_Zm ( node.bb_max.x, node.bb_max.y, node.bb_min.z );
00322
00323 const TPoint3Df p_Xm_Ym_Z0 ( node.bb_min.x, node.bb_min.y, node.center.z );
00324 const TPoint3Df p_X0_Ym_Z0 ( node.center.x, node.bb_min.y, node.center.z );
00325 const TPoint3Df p_Xp_Ym_Z0 ( node.bb_max.x, node.bb_min.y, node.center.z );
00326 const TPoint3Df p_Xm_Y0_Z0 ( node.bb_min.x, node.center.y, node.center.z );
00327 const TPoint3Df p_X0_Y0_Z0 ( node.center.x, node.center.y, node.center.z );
00328 const TPoint3Df p_Xp_Y0_Z0 ( node.bb_max.x, node.center.y, node.center.z );
00329 const TPoint3Df p_Xm_Yp_Z0 ( node.bb_min.x, node.bb_max.y, node.center.z );
00330 const TPoint3Df p_X0_Yp_Z0 ( node.center.x, node.bb_max.y, node.center.z );
00331 const TPoint3Df p_Xp_Yp_Z0 ( node.bb_max.x, node.bb_max.y, node.center.z );
00332
00333 const TPoint3Df p_Xm_Ym_Zp ( node.bb_min.x, node.bb_min.y, node.bb_max.z );
00334 const TPoint3Df p_X0_Ym_Zp ( node.center.x, node.bb_min.y, node.bb_max.z );
00335 const TPoint3Df p_Xp_Ym_Zp ( node.bb_min.x, node.bb_min.y, node.bb_max.z );
00336 const TPoint3Df p_Xm_Y0_Zp ( node.bb_min.x, node.center.y, node.bb_max.z );
00337 const TPoint3Df p_X0_Y0_Zp ( node.center.x, node.center.y, node.bb_max.z );
00338 const TPoint3Df p_Xp_Y0_Zp ( node.bb_max.x, node.center.y, node.bb_max.z );
00339 const TPoint3Df p_Xm_Yp_Zp ( node.bb_min.x, node.bb_max.y, node.bb_max.z );
00340 const TPoint3Df p_X0_Yp_Zp ( node.center.x, node.bb_max.y, node.bb_max.z );
00341 const TPoint3Df p_Xp_Yp_Zp ( node.bb_max.x, node.bb_max.y, node.bb_max.z );
00342
00343
00344 #define PROJ_SUB_NODE(POSTFIX) \
00345 TPixelCoordf px_##POSTFIX; \
00346 float depth_##POSTFIX; \
00347 ri.projectPointPixels( p_##POSTFIX.x, p_##POSTFIX.y, p_##POSTFIX.z, px_##POSTFIX.x,px_##POSTFIX.y,depth_##POSTFIX);
00348
00349 #define PROJ_SUB_NODE_ALREADY_DONE(INDEX, POSTFIX) \
00350 const TPixelCoordf px_##POSTFIX = cr_px[INDEX]; \
00351 float depth_##POSTFIX = cr_z[INDEX];
00352
00353 PROJ_SUB_NODE_ALREADY_DONE(0,Xm_Ym_Zm)
00354 PROJ_SUB_NODE(X0_Ym_Zm)
00355 PROJ_SUB_NODE_ALREADY_DONE(1, Xp_Ym_Zm)
00356
00357 PROJ_SUB_NODE(Xm_Y0_Zm)
00358 PROJ_SUB_NODE(X0_Y0_Zm)
00359 PROJ_SUB_NODE(Xp_Y0_Zm)
00360
00361 PROJ_SUB_NODE_ALREADY_DONE(2, Xm_Yp_Zm)
00362 PROJ_SUB_NODE(X0_Yp_Zm)
00363 PROJ_SUB_NODE_ALREADY_DONE(3, Xp_Yp_Zm)
00364
00365 PROJ_SUB_NODE(Xm_Ym_Z0)
00366 PROJ_SUB_NODE(X0_Ym_Z0)
00367 PROJ_SUB_NODE(Xp_Ym_Z0)
00368 PROJ_SUB_NODE(Xm_Y0_Z0)
00369 PROJ_SUB_NODE(X0_Y0_Z0)
00370 PROJ_SUB_NODE(Xp_Y0_Z0)
00371 PROJ_SUB_NODE(Xm_Yp_Z0)
00372 PROJ_SUB_NODE(X0_Yp_Z0)
00373 PROJ_SUB_NODE(Xp_Yp_Z0)
00374
00375 PROJ_SUB_NODE_ALREADY_DONE(4, Xm_Ym_Zp)
00376 PROJ_SUB_NODE(X0_Ym_Zp)
00377 PROJ_SUB_NODE_ALREADY_DONE(5, Xp_Ym_Zp)
00378
00379 PROJ_SUB_NODE(Xm_Y0_Zp)
00380 PROJ_SUB_NODE(X0_Y0_Zp)
00381 PROJ_SUB_NODE(Xp_Y0_Zp)
00382
00383 PROJ_SUB_NODE_ALREADY_DONE(6, Xm_Yp_Zp)
00384 PROJ_SUB_NODE(X0_Yp_Zp)
00385 PROJ_SUB_NODE_ALREADY_DONE(7, Xp_Yp_Zp)
00386
00387
00388 #define DO_RECURSE_CHILD(INDEX, SEQ0,SEQ1,SEQ2,SEQ3,SEQ4,SEQ5,SEQ6,SEQ7) \
00389 { \
00390 TPixelCoordf child_cr_px[8] = { px_##SEQ0,px_##SEQ1,px_##SEQ2,px_##SEQ3,px_##SEQ4,px_##SEQ5,px_##SEQ6,px_##SEQ7 }; \
00391 float child_cr_z[8] = { depth_##SEQ0,depth_##SEQ1,depth_##SEQ2,depth_##SEQ3,depth_##SEQ4,depth_##SEQ5,depth_##SEQ6,depth_##SEQ7 }; \
00392 this->octree_recursive_render(node.child_id[INDEX],ri,child_cr_px, child_cr_z); \
00393 }
00394
00395
00396 DO_RECURSE_CHILD(0, Xm_Ym_Zm, X0_Ym_Zm, Xm_Y0_Zm, X0_Y0_Zm, Xm_Ym_Z0, X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0 )
00397 DO_RECURSE_CHILD(1, X0_Ym_Zm, Xp_Ym_Zm, X0_Y0_Zm, Xp_Y0_Zm, X0_Ym_Z0, Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0 )
00398 DO_RECURSE_CHILD(2, Xm_Y0_Zm, X0_Y0_Zm, Xm_Yp_Zm, X0_Yp_Zm, Xm_Y0_Z0, X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0 )
00399 DO_RECURSE_CHILD(3, X0_Y0_Zm, Xp_Y0_Zm, X0_Yp_Zm, Xp_Yp_Zm, X0_Y0_Z0, Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0 )
00400 DO_RECURSE_CHILD(4, Xm_Ym_Z0, X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0, Xm_Ym_Zp, X0_Ym_Zp, Xm_Y0_Zp, X0_Y0_Zp )
00401 DO_RECURSE_CHILD(5, X0_Ym_Z0, Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0, X0_Ym_Zp, Xp_Ym_Zp, X0_Y0_Zp, Xp_Y0_Zp )
00402 DO_RECURSE_CHILD(6, Xm_Y0_Z0, X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0, Xm_Y0_Zp, X0_Y0_Zp, Xm_Yp_Zp, X0_Yp_Zp )
00403 DO_RECURSE_CHILD(7, X0_Y0_Z0, Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0, X0_Y0_Zp, Xp_Y0_Zp, X0_Yp_Zp, Xp_Yp_Zp )
00404 #undef DO_RECURSE_CHILD
00405 #undef PROJ_SUB_NODE
00406 #undef PROJ_SUB_NODE_ALREADY_DONE
00407 }
00408 }
00409 }
00410
00411
00412 void internal_octree_assure_uptodate()
00413 {
00414 if (!m_octree_has_to_rebuild_all) return;
00415 m_octree_has_to_rebuild_all = false;
00416
00417
00418 m_octree_nodes.assign(1, TNode() );
00419
00420
00421 internal_recursive_split( OCTREE_ROOT_NODE, true );
00422 }
00423
00424
00425
00426
00427 void internal_recursive_split(const size_t node_id, const bool all_pts = false)
00428 {
00429 TNode &node = m_octree_nodes[node_id];
00430 const size_t N = all_pts ? octree_derived().size() : node.pts.size();
00431
00432 const bool has_to_compute_bb = (node_id ==OCTREE_ROOT_NODE);
00433
00434 if (N<=mrpt::global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE)
00435 {
00436
00437 node.is_leaf = true;
00438 node.all = all_pts;
00439
00440
00441 if (has_to_compute_bb)
00442 {
00443 if (all_pts)
00444 for (size_t i=0;i<N;i++) node.update_bb( octree_derived().getPointf(i) );
00445 else for (size_t i=0;i<N;i++) node.update_bb( octree_derived().getPointf(node.pts[i]) );
00446 }
00447 }
00448 else
00449 {
00450
00451
00452 mrpt::math::TPoint3Df mean(0,0,0);
00453 if (all_pts)
00454 for (size_t i=0;i<N;i++)
00455 {
00456 mrpt::math::TPoint3Df p = octree_derived().getPointf(i);
00457 mean+= p;
00458 if (has_to_compute_bb) node.update_bb( p );
00459 }
00460 else
00461 for (size_t i=0;i<N;i++)
00462 {
00463 mrpt::math::TPoint3Df p = octree_derived().getPointf(node.pts[i]);
00464 mean+= p;
00465 if (has_to_compute_bb) node.update_bb( p );
00466 }
00467
00468
00469 node.is_leaf = false;
00470 node.center = mean * (1.0f/N);
00471
00472
00473 const size_t children_idx_base = m_octree_nodes.size();
00474 m_octree_nodes.resize(children_idx_base + 8 );
00475 for (int i=0;i<8;i++)
00476 node.child_id[i] = children_idx_base + i;
00477
00478
00479 for (int i=0;i<8;i++)
00480 m_octree_nodes[children_idx_base + i].setBBFromOrderInParent(node,i);
00481
00482
00483 const mrpt::math::TPoint3Df &c = node.center;
00484 for (size_t j=0;j<N;j++)
00485 {
00486 const size_t i = all_pts ? j : node.pts[j];
00487 const TPoint3Df p = octree_derived().getPointf(i);
00488 if (p.z<c.z)
00489 {
00490 if (p.y<c.y)
00491 {
00492 if (p.x<c.x)
00493 m_octree_nodes[children_idx_base+ 0 ].pts.push_back(i);
00494 else m_octree_nodes[children_idx_base+ 1 ].pts.push_back(i);
00495 }
00496 else
00497 {
00498 if (p.x<c.x)
00499 m_octree_nodes[children_idx_base+ 2 ].pts.push_back(i);
00500 else m_octree_nodes[children_idx_base+ 3 ].pts.push_back(i);
00501 }
00502 }
00503 else
00504 {
00505 if (p.y<c.y)
00506 {
00507 if (p.x<c.x)
00508 m_octree_nodes[children_idx_base+ 4 ].pts.push_back(i);
00509 else m_octree_nodes[children_idx_base+ 5 ].pts.push_back(i);
00510 }
00511 else
00512 {
00513 if (p.x<c.x)
00514 m_octree_nodes[children_idx_base+ 6 ].pts.push_back(i);
00515 else m_octree_nodes[children_idx_base+ 7 ].pts.push_back(i);
00516 }
00517 }
00518 }
00519
00520
00521 {
00522 std::vector<size_t> emptyVec;
00523 node.pts.swap(emptyVec);
00524 }
00525
00526
00527 for (int i=0;i<8;i++)
00528 internal_recursive_split( node.child_id[i] );
00529 }
00530 }
00531
00532 public:
00533
00534
00535 size_t octree_get_node_count() const { return m_octree_nodes.size(); }
00536
00537
00538 size_t octree_get_visible_nodes() const { return m_visible_octree_nodes; }
00539
00540
00541 inline void octree_mark_as_outdated() { m_octree_has_to_rebuild_all=true; }
00542
00543
00544
00545 void octree_get_graphics_boundingboxes(
00546 mrpt::opengl::CSetOfObjects &gl_bb,
00547 const double lines_width = 1,
00548 const TColorf lines_color = TColorf(1,1,1) ) const
00549 {
00550 octree_assure_uptodate();
00551 gl_bb.clear();
00552 for (size_t i=0;i<m_octree_nodes.size();i++)
00553 {
00554 const TNode & node = m_octree_nodes[i];
00555 if (!node.is_leaf) continue;
00556 mrpt::opengl::CBoxPtr gl_box = mrpt::opengl::CBox::Create();
00557 gl_box->setBoxCorners( mrpt::math::TPoint3D(node.bb_min), mrpt::math::TPoint3D(node.bb_max) );
00558 gl_box->setColor(lines_color);
00559 gl_box->setLineWidth(lines_width);
00560 gl_box->setWireframe(true);
00561 gl_bb.insert(gl_box);
00562 }
00563 }
00564
00565
00566
00567 void octree_debug_dump_tree(std::ostream &o) const
00568 {
00569 o << "Octree nodes: " << m_octree_nodes.size() << std::endl;
00570 size_t total_elements = 0;
00571 for (size_t i=0;i<m_octree_nodes.size();i++)
00572 {
00573 const TNode & node = m_octree_nodes[i];
00574
00575 o << "Node #" << i << ": ";
00576 if (node.is_leaf)
00577 {
00578 o << "leaf, ";
00579 if (node.all) { o << "(all)\n"; total_elements+=octree_derived().size(); }
00580 else { o << node.pts.size() << " elements; "; total_elements+=node.pts.size(); }
00581
00582 }
00583 else
00584 {
00585 o << "parent, center=(" << node.center.x << "," << node.center.y<<","<<node.center.z<<"), children: "
00586 << node.child_id[0] << ","<< node.child_id[1] << ","<< node.child_id[2] << ","<< node.child_id[3] << ","
00587 << node.child_id[4] << ","<< node.child_id[5] << ","<< node.child_id[6] << ","<< node.child_id[7] << "; ";
00588 }
00589 o << " bb: (" << node.bb_min.x << ","<< node.bb_min.y << ","<< node.bb_min.z << ")-("
00590 << node.bb_max.x << ","<< node.bb_max.y << ","<< node.bb_max.z << ")\n";
00591 }
00592 o << "Total elements in all nodes: " << total_elements << std::endl;
00593 }
00594
00595 };
00596
00597 }
00598 }
00599 #endif