rtcVertexOctree.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2008
 * Robert Bosch LLC
 * Research and Technology Center North America
 * Palo Alto, California
 *
 * All rights reserved.
 *
 *------------------------------------------------------------------------------
 * project ....: PUMA: Probablistic Unsupervised Model Acquisition
 * file .......: VertexOctree.cpp
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 07/25/2006
 * modified ...: $Date: 2009-09-07 14:07:53 -0700 (Mon, 07 Sep 2009) $
 * changed by .: $Author: benjaminpitzer $
 * revision ...: $Revision: 904 $
 */

//== INCLUDES ==================================================================
#include <BoundingBox.h>
#include "VertexOctree.h"

//== NAMESPACES ================================================================
using namespace std;
namespace puma {

//== IMPLEMENTATION ============================================================
VertexOctree::VertexOctree(
  const TriMesh*   mesh,
  unsigned int     maximum_depth,
  unsigned int     min_entries)
  : m_mesh(mesh), m_current_depth(0), m_maximum_depth(maximum_depth), m_min_entries(min_entries)
{
  // calculate bounding box
  Point bbMin, bbMax;
  BoundingBox bb;
  bb.apply(*mesh,bbMin,bbMax);

  // get vertices from mesh
  ConstVertexIter vIt(mesh->vertices_begin());
  ConstVertexIter vEnd(mesh->vertices_end());
  vector<VertexHandle> vertices;
  vertices.reserve(mesh->n_vertices());
  for (; vIt!=vEnd; ++vIt) {
    if(mesh->point(vIt)[2]<2.5)
    vertices.push_back(vIt.handle());
  }

  // set members
  m_center = point_to_vec((bbMax+bbMin)/(float)2.0);
  m_radius = bbMax.max();
  m_parentIndex = 0;
  m_parentTree = NULL;

  // initialize octree
  initialize(vertices);
}

VertexOctree::VertexOctree(
  const TriMesh*   mesh,
  Vec3f   center,
  float  radius,
  unsigned int     maximum_depth,
  unsigned int     min_entries)
  : m_mesh(mesh), m_center(center), m_radius(radius), m_current_depth(0), m_maximum_depth(maximum_depth), m_min_entries(min_entries)
{
  // get vertices from mesh
  ConstVertexIter vIt(mesh->vertices_begin());
  ConstVertexIter vEnd(mesh->vertices_end());
  vector<VertexHandle> vertices;
  vertices.reserve(mesh->n_vertices());
  for (; vIt!=vEnd; ++vIt) {
    vertices.push_back(vIt.handle());
  }

  // set members
  m_parentIndex = 0;
  m_parentTree = NULL;

  // initialize octree
  initialize(vertices);
}

VertexOctree::VertexOctree(
  const TriMesh*           mesh,
  Vec3f                    center,
  float                    radius,
  unsigned int             current_depth,
  std::vector<VertexHandle>& vertices,
  unsigned int             maximum_depth,
  unsigned int             min_entries,
  int                      parentIndex,
  VertexOctree*            parentTree) :
  m_mesh(mesh), m_center(center), m_radius(radius), m_parentIndex(parentIndex),
  m_parentTree(parentTree), m_current_depth(current_depth), m_maximum_depth(maximum_depth), m_min_entries(min_entries)
{
  initialize(vertices);
}

void VertexOctree::
initialize(vector<VertexHandle>& vertices)
{
  bool subdivided=false;
  Vec3f vert;
  m_vertices.clear();
  m_isLeaf = false;

  // check whether we need to subdivide more
  if (vertices.size() <= m_min_entries || m_current_depth >= m_maximum_depth) {
    m_isLeaf = true;
    m_vertices = vertices;
    m_child[0] = m_child[1] = m_child[2] = m_child[3] = NULL;
    m_child[4] = m_child[5] = m_child[6] = m_child[7] = NULL;
    return;
  }

  // if so, try for triangle and each child whether they intersect
  float halfR = m_radius*.5;
  Vec3f childCtr(0.0);

  vector<VertexHandle> xp, xm; // x plus, x minus
  vector<VertexHandle> yp, ym; // y plus, y minus
  vector<VertexHandle> zp, zm; // z plus, z minus
  xp.reserve(vertices.size());
  xm.reserve(vertices.size());

  // split about x axis
  // find closest vertex
  vector<VertexHandle>::const_iterator vIt, vIt_end;
  for(vIt=vertices.begin(), vIt_end=vertices.end(); vIt!=vIt_end; ++vIt)
  {
    vert = point_to_vec(m_mesh->point(*vIt));
    if (vert[0] >= m_center[0]) {
      xp.push_back(*vIt);
    } else {
      xm.push_back(*vIt);
    }
  }

  // split about y axis
  vector<VertexHandle> &xv = xm;
  vector<VertexHandle> &yv = ym;
  for (int i=0; i<2; i++) {
    childCtr[0] = m_center[0] + (i ? halfR : -halfR);
    if (i) xv = xp;
    else   xv = xm;
    yp.clear(); yp.reserve(xv.size());
    ym.clear(); ym.reserve(xv.size());

    for(vIt=xv.begin(), vIt_end=xv.end(); vIt!=vIt_end; ++vIt)
    {
      vert = point_to_vec(m_mesh->point(*vIt));
      if (vert[1] >= m_center[1]) {
        yp.push_back(*vIt);
      } else  {
        ym.push_back(*vIt);
      }
    }

    // split about z axis
    for (int j=0; j<2; j++) {
      childCtr[1] = m_center[1] + (j ? halfR : -halfR);
      if (j) yv = yp;
      else   yv = ym;
      zp.clear(); zp.reserve(yv.size());
      zm.clear(); zm.reserve(yv.size());

      for(vIt=yv.begin(), vIt_end=yv.end(); vIt!=vIt_end; ++vIt)
      {
        vert = point_to_vec(m_mesh->point(*vIt));
        if (vert[2] >= m_center[2]) {
          zp.push_back(*vIt);
        } else {
          zm.push_back(*vIt);
        }
      }

      // create the children, if needed
      childCtr[2] = m_center[2] - halfR;
      int k = i+2*j;
      if (zm.size()>=m_min_entries) {
        m_child[k] = new VertexOctree(m_mesh, childCtr, halfR, m_current_depth+1, zm, m_maximum_depth, m_min_entries, k, this);
        subdivided = true;
      } else {
        m_child[k] = NULL;
      }

      childCtr[2] = m_center[2] + halfR;
      k += 4;
      if (zp.size()>=m_min_entries) {
        m_child[k] = new VertexOctree(m_mesh, childCtr, halfR, m_current_depth+1, zp, m_maximum_depth, m_min_entries, k, this);
        subdivided = true;
      } else {
        m_child[k] = NULL;
      }
    }
  }

  // no subdivision made
  if(!subdivided)
  {
    m_vertices = vertices;
    m_isLeaf = true;
    m_child[0] = m_child[1] = m_child[2] = m_child[3] = NULL;
    m_child[4] = m_child[5] = m_child[6] = m_child[7] = NULL;
  }
}

void VertexOctree::
refineTree()
{
  if(m_isLeaf)
  {
    if(m_vertices.size()<=m_min_entries || m_current_depth >= m_maximum_depth)
      return;

    vector<VertexHandle> vertices = m_vertices;
    initialize(vertices);
    return;
  }

  for (int i=0; i<8; i++) {
    if (m_child[i])
      m_child[i]->refineTree();
  }
}


bool VertexOctree::
searchNearest(const Vec3f &p, VertexHandle& vertex, float &distance)
{
  // is this a leaf node?
  if (m_isLeaf) {
    // look for a new closest distance
    vector<VertexHandle>::const_iterator vIt, vIt_end;
    for(vIt=m_vertices.begin(), vIt_end=m_vertices.end(); vIt!=vIt_end; ++vIt)
    {
      // check if distance to othis triangle is smaller
      if(closerToVertex(p,*vIt,distance))
        vertex = *vIt;
    }
    return puma_ball_within_bounds(p, sqrtf(distance), m_center, m_radius);
  }

  // which child contains p?
  int iChild = 4*(m_center[2]<p[2]) + 2*(m_center[1]<p[1]) + (m_center[0]<p[0]);

  // check that child first
  if (m_child[iChild] && m_child[iChild]->searchNearest(p, vertex, distance)) return true;

  // now see if the other children need to be checked
  for (int i=0; i<8; i++) {
    if (i==iChild) continue;
    if (m_child[i] && puma_bounds_overlap_ball(p,sqrtf(distance),m_child[i]->m_center, m_child[i]->m_radius)) {
      if (m_child[i]->searchNearest(p, vertex, distance)) return true;
    }
  }
  return puma_ball_within_bounds(p, sqrtf(distance), m_center, m_radius);
}

bool VertexOctree::
closerToVertex(const Vec3f &p, const VertexHandle& vertex, float& distance)
{
  float distance_temp = puma_dist2(p,point_to_vec(m_mesh->point(vertex)));
  if (distance_temp < distance) {
    distance = distance_temp;
    return true;
  }
  else {
    return false;
  }
}

void VertexOctree::getPoints(vector<Vec3f> &p)
{
  if (m_isLeaf) {
    p.push_back(m_center + Vec3f(-m_radius,-m_radius,-m_radius));//0
    p.push_back(m_center + Vec3f( m_radius,-m_radius,-m_radius));//1
    p.push_back(m_center + Vec3f(-m_radius, m_radius,-m_radius));//2
    p.push_back(m_center + Vec3f( m_radius, m_radius,-m_radius));//3
    p.push_back(m_center + Vec3f(-m_radius,-m_radius, m_radius));//4
    p.push_back(m_center + Vec3f( m_radius,-m_radius, m_radius));//5
    p.push_back(m_center + Vec3f(-m_radius, m_radius, m_radius));//6
    p.push_back(m_center + Vec3f( m_radius, m_radius, m_radius));//7
    return;
  }
  for (int i=0; i<8; i++) {
    if (m_child[i])
      m_child[i]->getPoints(p);
  }

}

void VertexOctree::getCubes(vector<Vec3f> &p, vector<int> &edgeVertices)
{
  if (m_isLeaf && m_current_depth==m_maximum_depth) {
    int s = p.size();
    p.push_back(m_center + Vec3f(-m_radius,-m_radius,-m_radius));//0
    p.push_back(m_center + Vec3f( m_radius,-m_radius,-m_radius));//1
    p.push_back(m_center + Vec3f(-m_radius, m_radius,-m_radius));//2
    p.push_back(m_center + Vec3f( m_radius, m_radius,-m_radius));//3
    p.push_back(m_center + Vec3f(-m_radius,-m_radius, m_radius));//4
    p.push_back(m_center + Vec3f( m_radius,-m_radius, m_radius));//5
    p.push_back(m_center + Vec3f(-m_radius, m_radius, m_radius));//6
    p.push_back(m_center + Vec3f( m_radius, m_radius, m_radius));//7

    edgeVertices.push_back(s+0);edgeVertices.push_back(s+1);
    edgeVertices.push_back(s+0);edgeVertices.push_back(s+4);
    edgeVertices.push_back(s+1);edgeVertices.push_back(s+3);
    edgeVertices.push_back(s+2);edgeVertices.push_back(s+0);
    edgeVertices.push_back(s+3);edgeVertices.push_back(s+2);
    edgeVertices.push_back(s+4);edgeVertices.push_back(s+6);
    edgeVertices.push_back(s+6);edgeVertices.push_back(s+2);
    edgeVertices.push_back(s+6);edgeVertices.push_back(s+7);
    edgeVertices.push_back(s+7);edgeVertices.push_back(s+3);
    edgeVertices.push_back(s+7);edgeVertices.push_back(s+5);
    edgeVertices.push_back(s+5);edgeVertices.push_back(s+1);
    edgeVertices.push_back(s+5);edgeVertices.push_back(s+4);
    return;
  }
  for (int i=0; i<8; i++) {
    if (m_child[i])
      m_child[i]->getCubes(p,edgeVertices);
  }
}

void VertexOctree::getCubeCenters(vector<Vec3f> &centers)
{
  if (m_isLeaf) {
    centers.push_back(m_center);
    return;
  }
  for (int i=0; i<8; i++) {
    if (m_child[i])
      m_child[i]->getCubeCenters(centers);
  }
}

void VertexOctree::getVertices(vector<VertexHandle>& vertices)
{
  if (m_isLeaf) {
    int nf = m_vertices.size();
    for(int i=0;i<nf;++i)
      vertices.push_back(m_vertices[i]);
    return;
  }
  for (int i=0; i<8; i++) {
    if (m_child[i])
      m_child[i]->getVertices(vertices);
  }
}

void VertexOctree::getLeaves(vector<VertexOctree*>& leafes)
{
  if (m_isLeaf) {
    leafes.push_back(this);
    return;
  }
  for (int i=0; i<8; i++) {
    if (m_child[i])
      m_child[i]->getLeaves(leafes);
  }

}

//==============================================================================
} // namespace puma
//==============================================================================