rtcVertexOctree.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2008
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: PUMA: Probablistic Unsupervised Model Acquisition
00011  * file .......: VertexOctree.cpp
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 07/25/2006
00015  * modified ...: $Date: 2009-09-07 14:07:53 -0700 (Mon, 07 Sep 2009) $
00016  * changed by .: $Author: benjaminpitzer $
00017  * revision ...: $Revision: 904 $
00018  */
00019 
00020 //== INCLUDES ==================================================================
00021 #include <BoundingBox.h>
00022 #include "VertexOctree.h"
00023 
00024 //== NAMESPACES ================================================================
00025 using namespace std;
00026 namespace puma {
00027 
00028 //== IMPLEMENTATION ============================================================
00029 VertexOctree::VertexOctree(
00030   const TriMesh*   mesh,
00031   unsigned int     maximum_depth,
00032   unsigned int     min_entries)
00033   : m_mesh(mesh), m_current_depth(0), m_maximum_depth(maximum_depth), m_min_entries(min_entries)
00034 {
00035   // calculate bounding box
00036   Point bbMin, bbMax;
00037   BoundingBox bb;
00038   bb.apply(*mesh,bbMin,bbMax);
00039 
00040   // get vertices from mesh
00041   ConstVertexIter vIt(mesh->vertices_begin());
00042   ConstVertexIter vEnd(mesh->vertices_end());
00043   vector<VertexHandle> vertices;
00044   vertices.reserve(mesh->n_vertices());
00045   for (; vIt!=vEnd; ++vIt) {
00046     if(mesh->point(vIt)[2]<2.5)
00047     vertices.push_back(vIt.handle());
00048   }
00049 
00050   // set members
00051   m_center = point_to_vec((bbMax+bbMin)/(float)2.0);
00052   m_radius = bbMax.max();
00053   m_parentIndex = 0;
00054   m_parentTree = NULL;
00055 
00056   // initialize octree
00057   initialize(vertices);
00058 }
00059 
00060 VertexOctree::VertexOctree(
00061   const TriMesh*   mesh,
00062   Vec3f   center,
00063   float  radius,
00064   unsigned int     maximum_depth,
00065   unsigned int     min_entries)
00066   : m_mesh(mesh), m_center(center), m_radius(radius), m_current_depth(0), m_maximum_depth(maximum_depth), m_min_entries(min_entries)
00067 {
00068   // get vertices from mesh
00069   ConstVertexIter vIt(mesh->vertices_begin());
00070   ConstVertexIter vEnd(mesh->vertices_end());
00071   vector<VertexHandle> vertices;
00072   vertices.reserve(mesh->n_vertices());
00073   for (; vIt!=vEnd; ++vIt) {
00074     vertices.push_back(vIt.handle());
00075   }
00076 
00077   // set members
00078   m_parentIndex = 0;
00079   m_parentTree = NULL;
00080 
00081   // initialize octree
00082   initialize(vertices);
00083 }
00084 
00085 VertexOctree::VertexOctree(
00086   const TriMesh*           mesh,
00087   Vec3f                    center,
00088   float                    radius,
00089   unsigned int             current_depth,
00090   std::vector<VertexHandle>& vertices,
00091   unsigned int             maximum_depth,
00092   unsigned int             min_entries,
00093   int                      parentIndex,
00094   VertexOctree*            parentTree) :
00095   m_mesh(mesh), m_center(center), m_radius(radius), m_parentIndex(parentIndex),
00096   m_parentTree(parentTree), m_current_depth(current_depth), m_maximum_depth(maximum_depth), m_min_entries(min_entries)
00097 {
00098   initialize(vertices);
00099 }
00100 
00101 void VertexOctree::
00102 initialize(vector<VertexHandle>& vertices)
00103 {
00104   bool subdivided=false;
00105   Vec3f vert;
00106   m_vertices.clear();
00107   m_isLeaf = false;
00108 
00109   // check whether we need to subdivide more
00110   if (vertices.size() <= m_min_entries || m_current_depth >= m_maximum_depth) {
00111     m_isLeaf = true;
00112     m_vertices = vertices;
00113     m_child[0] = m_child[1] = m_child[2] = m_child[3] = NULL;
00114     m_child[4] = m_child[5] = m_child[6] = m_child[7] = NULL;
00115     return;
00116   }
00117 
00118   // if so, try for triangle and each child whether they intersect
00119   float halfR = m_radius*.5;
00120   Vec3f childCtr(0.0);
00121 
00122   vector<VertexHandle> xp, xm; // x plus, x minus
00123   vector<VertexHandle> yp, ym; // y plus, y minus
00124   vector<VertexHandle> zp, zm; // z plus, z minus
00125   xp.reserve(vertices.size());
00126   xm.reserve(vertices.size());
00127 
00128   // split about x axis
00129   // find closest vertex
00130   vector<VertexHandle>::const_iterator vIt, vIt_end;
00131   for(vIt=vertices.begin(), vIt_end=vertices.end(); vIt!=vIt_end; ++vIt)
00132   {
00133     vert = point_to_vec(m_mesh->point(*vIt));
00134     if (vert[0] >= m_center[0]) {
00135       xp.push_back(*vIt);
00136     } else {
00137       xm.push_back(*vIt);
00138     }
00139   }
00140 
00141   // split about y axis
00142   vector<VertexHandle> &xv = xm;
00143   vector<VertexHandle> &yv = ym;
00144   for (int i=0; i<2; i++) {
00145     childCtr[0] = m_center[0] + (i ? halfR : -halfR);
00146     if (i) xv = xp;
00147     else   xv = xm;
00148     yp.clear(); yp.reserve(xv.size());
00149     ym.clear(); ym.reserve(xv.size());
00150 
00151     for(vIt=xv.begin(), vIt_end=xv.end(); vIt!=vIt_end; ++vIt)
00152     {
00153       vert = point_to_vec(m_mesh->point(*vIt));
00154       if (vert[1] >= m_center[1]) {
00155         yp.push_back(*vIt);
00156       } else  {
00157         ym.push_back(*vIt);
00158       }
00159     }
00160 
00161     // split about z axis
00162     for (int j=0; j<2; j++) {
00163       childCtr[1] = m_center[1] + (j ? halfR : -halfR);
00164       if (j) yv = yp;
00165       else   yv = ym;
00166       zp.clear(); zp.reserve(yv.size());
00167       zm.clear(); zm.reserve(yv.size());
00168 
00169       for(vIt=yv.begin(), vIt_end=yv.end(); vIt!=vIt_end; ++vIt)
00170       {
00171         vert = point_to_vec(m_mesh->point(*vIt));
00172         if (vert[2] >= m_center[2]) {
00173           zp.push_back(*vIt);
00174         } else {
00175           zm.push_back(*vIt);
00176         }
00177       }
00178 
00179       // create the children, if needed
00180       childCtr[2] = m_center[2] - halfR;
00181       int k = i+2*j;
00182       if (zm.size()>=m_min_entries) {
00183         m_child[k] = new VertexOctree(m_mesh, childCtr, halfR, m_current_depth+1, zm, m_maximum_depth, m_min_entries, k, this);
00184         subdivided = true;
00185       } else {
00186         m_child[k] = NULL;
00187       }
00188 
00189       childCtr[2] = m_center[2] + halfR;
00190       k += 4;
00191       if (zp.size()>=m_min_entries) {
00192         m_child[k] = new VertexOctree(m_mesh, childCtr, halfR, m_current_depth+1, zp, m_maximum_depth, m_min_entries, k, this);
00193         subdivided = true;
00194       } else {
00195         m_child[k] = NULL;
00196       }
00197     }
00198   }
00199 
00200   // no subdivision made
00201   if(!subdivided)
00202   {
00203     m_vertices = vertices;
00204     m_isLeaf = true;
00205     m_child[0] = m_child[1] = m_child[2] = m_child[3] = NULL;
00206     m_child[4] = m_child[5] = m_child[6] = m_child[7] = NULL;
00207   }
00208 }
00209 
00210 void VertexOctree::
00211 refineTree()
00212 {
00213   if(m_isLeaf)
00214   {
00215     if(m_vertices.size()<=m_min_entries || m_current_depth >= m_maximum_depth)
00216       return;
00217 
00218     vector<VertexHandle> vertices = m_vertices;
00219     initialize(vertices);
00220     return;
00221   }
00222 
00223   for (int i=0; i<8; i++) {
00224     if (m_child[i])
00225       m_child[i]->refineTree();
00226   }
00227 }
00228 
00229 
00230 bool VertexOctree::
00231 searchNearest(const Vec3f &p, VertexHandle& vertex, float &distance)
00232 {
00233   // is this a leaf node?
00234   if (m_isLeaf) {
00235     // look for a new closest distance
00236     vector<VertexHandle>::const_iterator vIt, vIt_end;
00237     for(vIt=m_vertices.begin(), vIt_end=m_vertices.end(); vIt!=vIt_end; ++vIt)
00238     {
00239       // check if distance to othis triangle is smaller
00240       if(closerToVertex(p,*vIt,distance))
00241         vertex = *vIt;
00242     }
00243     return puma_ball_within_bounds(p, sqrtf(distance), m_center, m_radius);
00244   }
00245 
00246   // which child contains p?
00247   int iChild = 4*(m_center[2]<p[2]) + 2*(m_center[1]<p[1]) + (m_center[0]<p[0]);
00248 
00249   // check that child first
00250   if (m_child[iChild] && m_child[iChild]->searchNearest(p, vertex, distance)) return true;
00251 
00252   // now see if the other children need to be checked
00253   for (int i=0; i<8; i++) {
00254     if (i==iChild) continue;
00255     if (m_child[i] && puma_bounds_overlap_ball(p,sqrtf(distance),m_child[i]->m_center, m_child[i]->m_radius)) {
00256       if (m_child[i]->searchNearest(p, vertex, distance)) return true;
00257     }
00258   }
00259   return puma_ball_within_bounds(p, sqrtf(distance), m_center, m_radius);
00260 }
00261 
00262 bool VertexOctree::
00263 closerToVertex(const Vec3f &p, const VertexHandle& vertex, float& distance)
00264 {
00265   float distance_temp = puma_dist2(p,point_to_vec(m_mesh->point(vertex)));
00266   if (distance_temp < distance) {
00267     distance = distance_temp;
00268     return true;
00269   }
00270   else {
00271     return false;
00272   }
00273 }
00274 
00275 void VertexOctree::getPoints(vector<Vec3f> &p)
00276 {
00277   if (m_isLeaf) {
00278     p.push_back(m_center + Vec3f(-m_radius,-m_radius,-m_radius));//0
00279     p.push_back(m_center + Vec3f( m_radius,-m_radius,-m_radius));//1
00280     p.push_back(m_center + Vec3f(-m_radius, m_radius,-m_radius));//2
00281     p.push_back(m_center + Vec3f( m_radius, m_radius,-m_radius));//3
00282     p.push_back(m_center + Vec3f(-m_radius,-m_radius, m_radius));//4
00283     p.push_back(m_center + Vec3f( m_radius,-m_radius, m_radius));//5
00284     p.push_back(m_center + Vec3f(-m_radius, m_radius, m_radius));//6
00285     p.push_back(m_center + Vec3f( m_radius, m_radius, m_radius));//7
00286     return;
00287   }
00288   for (int i=0; i<8; i++) {
00289     if (m_child[i])
00290       m_child[i]->getPoints(p);
00291   }
00292 
00293 }
00294 
00295 void VertexOctree::getCubes(vector<Vec3f> &p, vector<int> &edgeVertices)
00296 {
00297   if (m_isLeaf && m_current_depth==m_maximum_depth) {
00298     int s = p.size();
00299     p.push_back(m_center + Vec3f(-m_radius,-m_radius,-m_radius));//0
00300     p.push_back(m_center + Vec3f( m_radius,-m_radius,-m_radius));//1
00301     p.push_back(m_center + Vec3f(-m_radius, m_radius,-m_radius));//2
00302     p.push_back(m_center + Vec3f( m_radius, m_radius,-m_radius));//3
00303     p.push_back(m_center + Vec3f(-m_radius,-m_radius, m_radius));//4
00304     p.push_back(m_center + Vec3f( m_radius,-m_radius, m_radius));//5
00305     p.push_back(m_center + Vec3f(-m_radius, m_radius, m_radius));//6
00306     p.push_back(m_center + Vec3f( m_radius, m_radius, m_radius));//7
00307 
00308     edgeVertices.push_back(s+0);edgeVertices.push_back(s+1);
00309     edgeVertices.push_back(s+0);edgeVertices.push_back(s+4);
00310     edgeVertices.push_back(s+1);edgeVertices.push_back(s+3);
00311     edgeVertices.push_back(s+2);edgeVertices.push_back(s+0);
00312     edgeVertices.push_back(s+3);edgeVertices.push_back(s+2);
00313     edgeVertices.push_back(s+4);edgeVertices.push_back(s+6);
00314     edgeVertices.push_back(s+6);edgeVertices.push_back(s+2);
00315     edgeVertices.push_back(s+6);edgeVertices.push_back(s+7);
00316     edgeVertices.push_back(s+7);edgeVertices.push_back(s+3);
00317     edgeVertices.push_back(s+7);edgeVertices.push_back(s+5);
00318     edgeVertices.push_back(s+5);edgeVertices.push_back(s+1);
00319     edgeVertices.push_back(s+5);edgeVertices.push_back(s+4);
00320     return;
00321   }
00322   for (int i=0; i<8; i++) {
00323     if (m_child[i])
00324       m_child[i]->getCubes(p,edgeVertices);
00325   }
00326 }
00327 
00328 void VertexOctree::getCubeCenters(vector<Vec3f> &centers)
00329 {
00330   if (m_isLeaf) {
00331     centers.push_back(m_center);
00332     return;
00333   }
00334   for (int i=0; i<8; i++) {
00335     if (m_child[i])
00336       m_child[i]->getCubeCenters(centers);
00337   }
00338 }
00339 
00340 void VertexOctree::getVertices(vector<VertexHandle>& vertices)
00341 {
00342   if (m_isLeaf) {
00343     int nf = m_vertices.size();
00344     for(int i=0;i<nf;++i)
00345       vertices.push_back(m_vertices[i]);
00346     return;
00347   }
00348   for (int i=0; i<8; i++) {
00349     if (m_child[i])
00350       m_child[i]->getVertices(vertices);
00351   }
00352 }
00353 
00354 void VertexOctree::getLeaves(vector<VertexOctree*>& leafes)
00355 {
00356   if (m_isLeaf) {
00357     leafes.push_back(this);
00358     return;
00359   }
00360   for (int i=0; i<8; i++) {
00361     if (m_child[i])
00362       m_child[i]->getLeaves(leafes);
00363   }
00364 
00365 }
00366 
00367 //==============================================================================
00368 } // namespace puma
00369 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Mon Oct 6 2014 10:07:35