Boctree.h

Go to the documentation of this file.

#ifndef BOCTREE_H
#define BOCTREE_H

#include "searchTree.h"
#include "point_type.h"
#include "data_types.h"
#include "allocator.h"
#include "limits.h"
#include "nnparams.h"
#include "globals.icc"


#include <stdio.h>

#include <vector>
using std::vector;
#include <deque>
using std::deque;
#include <set>
using std::set;
#include <list>
using std::list;
#include <iostream>
#include <fstream>
#include <string>

#if __GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)
  #define POPCOUNT(mask) __builtin_popcount(mask)
#else
  #define POPCOUNT(mask) _my_popcount_3(mask)
#endif

#include <boost/interprocess/offset_ptr.hpp> // to avoid ifdeffing for offset_ptr.get(), use &(*ptr)
namespace { namespace ip = boost::interprocess; }


// forward declaration
template <class T> union bitunion;

template <class T> union dunion {
  T v;
  unsigned int    length;
  dunion() : length(0) {};

};
// typedefs in combination with templates are weird
//typedef dunion<T> pointrep<T>;
#define pointrep union dunion<T>




class bitoct{
  public:

#ifdef _MSC_VER
  __int64 child_pointer        : 48;
  unsigned valid              :  8;
  unsigned leaf               :  8;
#else
  signed long child_pointer   : 48;
  unsigned valid              :  8;
  unsigned leaf               :  8;
#endif

  template <class T>
  static inline void link(bitoct &parent, bitunion<T> *child) {
    parent.child_pointer = (long)((char*)child - (char*)&parent);
  }

  template <class T>
  static inline void getChildren(const bitoct &parent, bitunion<T>* &children) {
    children = (bitunion<T>*)((char*)&parent + parent.child_pointer);
  }

  template <class T>
  inline bitunion<T>* getChild(unsigned char index) {
    bitunion<T> *children = (bitunion<T>*)((char*)this + this->child_pointer);
    for (unsigned char i = 0; i < index; i++) {
      if (  ( 1 << i ) & valid ) {   // if ith node exists
        children++;
      }
    }
    return children;
  }
};
  

template <class T> union bitunion {
  pointrep *points;
  //union dunion<T> *points;
  bitoct node;

  bitunion(pointrep *p) : points(p) {};
  bitunion(bitoct b) : node(b) {};
  bitunion() : points(0) {
    node.child_pointer = 0;
    node.valid = 0;
    node.leaf = 0;
  };           // needed for new []
  
  static inline void link(bitunion<T>* leaf, pointrep* points) {
    // use node child_pointer as offset_ptr, not pointrep
    leaf->node.child_pointer = (long)((char*)points - (char*)leaf);
  }
  
  inline T* getPoints() const {
    // absolute pointer
    //return &(this->points[1].v);
    // offset pointer
    return reinterpret_cast<T*>(
      reinterpret_cast<pointrep*>((char*)this + node.child_pointer) + 1
    );
  }
  
  inline unsigned int getLength() const {
    // absolute pointer
    //return this->points[0].length;
    // offset pointer
    return (reinterpret_cast<pointrep*>((char*)this + node.child_pointer))[0].length;
  }
  
  inline pointrep* getPointreps() const {
    return reinterpret_cast<pointrep*>((char*)this + node.child_pointer);
  }
  
  inline bitunion<T>* getChild(unsigned char index) const {
    bitunion<T> *children = (bitunion<T>*)((char*)this + this->node.child_pointer);
    for (unsigned char i = 0; i < index; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        children++;
      }
    }
    return children;
  }
  
  inline bool isValid(unsigned char index) {
    return  (  ( 1 << index ) & node.valid );
  }
 /*
  inline pointrep* getChild(unsigned char index) {
    bitunion<T> *children = (bitunion<T>*)((char*)this + this->node.child_pointer);
    return children[index].points; 
  }*/
  
  inline bool childIsLeaf(unsigned char index) {
    return (  ( 1 << index ) & node.leaf ); // if ith node is leaf get center
  }
};


// initialized in Boctree.cc, sequence intialized on startup
extern char amap[8][8];
extern char imap[8][8];
extern char sequence2ci[8][256][8];  // maps preference to index in children array for every valid_mask and every case



template <typename T>
class BOctTree : public SearchTree {
public:
  BOctTree() {
  }

  template <class P>
  BOctTree(P * const* pts, int n, T voxelSize, PointType _pointtype = PointType(), bool _earlystop = false ) : pointtype(_pointtype), earlystop(_earlystop)
  {
    alloc = new PackedChunkAllocator;
    
    this->voxelSize = voxelSize;

    this->POINTDIM = pointtype.getPointDim();

    mins = alloc->allocate<T>(POINTDIM);
    maxs = alloc->allocate<T>(POINTDIM);

    // initialising
    for (unsigned int i = 0; i < POINTDIM; i++) { 
      mins[i] = pts[0][i]; 
      maxs[i] = pts[0][i];
    }

    for (unsigned int i = 0; i < POINTDIM; i++) { 
      for (int j = 1; j < n; j++) {
        mins[i] = min(mins[i], (T)pts[j][i]);
        maxs[i] = max(maxs[i], (T)pts[j][i]);
      }
    }

    center[0] = 0.5 * (mins[0] + maxs[0]);
    center[1] = 0.5 * (mins[1] + maxs[1]);
    center[2] = 0.5 * (mins[2] + maxs[2]);
    size = max(max(0.5 * (maxs[0] - mins[0]), 0.5 * (maxs[1] - mins[1])), 0.5 * (maxs[2] - mins[2]));
    size += 1.0; // for numerical reasons we increase size 

    // calculate new buckets
    T newcenter[8][3];
    T sizeNew = size / 2.0;

    for (unsigned char i = 0; i < 8; i++) {
      childcenter(center, newcenter[i], size, i);
    }
    // set up values
    uroot = alloc->allocate<bitunion<T> >();    
    root = &uroot->node;

    countPointsAndQueueFast(pts, n, newcenter, sizeNew, *root, center);
    init();
  }

  BOctTree(std::string filename) {
    alloc = new PackedChunkAllocator;
    deserialize(filename); 
    init();
  }

  template <class P>
  BOctTree(vector<P *> &pts, T voxelSize, PointType _pointtype = PointType(), bool _earlystop = false) : earlystop(_earlystop)
  {
    alloc = new PackedChunkAllocator;
    
    this->voxelSize = voxelSize;

    this->POINTDIM = pointtype.getPointDim();

    mins = alloc->allocate<T>(POINTDIM);
    maxs = alloc->allocate<T>(POINTDIM);

    // initialising
    for (unsigned int i = 0; i < POINTDIM; i++) { 
      mins[i] = pts[0][i]; 
      maxs[i] = pts[0][i];
    }

    for (unsigned int i = 0; i < POINTDIM; i++) { 
      for (unsigned int j = 1; j < pts.size(); j++) {
        mins[i] = min(mins[i], pts[j][i]);
        maxs[i] = max(maxs[i], pts[j][i]);
      }
    }

    center[0] = 0.5 * (mins[0] + maxs[0]);
    center[1] = 0.5 * (mins[1] + maxs[1]);
    center[2] = 0.5 * (mins[2] + maxs[2]);
    size = max(max(0.5 * (maxs[0] - mins[0]), 0.5 * (maxs[1] - mins[1])), 0.5 * (maxs[2] - mins[2]));
    
    size += 1.0; // for numerical reasons we increase size 

    // calculate new buckets
    T newcenter[8][3];
    T sizeNew = size / 2.0;

    for (unsigned char i = 0; i < 8; i++) {
      childcenter(center, newcenter[i], size, i);
    }
    // set up values
    uroot = alloc->allocate<bitunion<T> >();    
    root = &uroot->node;

    countPointsAndQueue(pts, newcenter, sizeNew, *root, center);
  }

  virtual ~BOctTree()
  {
    if(alloc) {
      delete alloc;
    }
  } 

  void init() {
    // compute maximal depth as well as the size of the smalles leaf
    real_voxelSize = size;
    max_depth = 1;
    while (real_voxelSize > voxelSize) {
      real_voxelSize = real_voxelSize/2.0;
      max_depth++;
    }
    
    child_bit_depth = alloc->allocate<unsigned int>(max_depth);
    child_bit_depth_inv = alloc->allocate<unsigned int>(max_depth);

    for(int d=0; d < max_depth; d++) {
      child_bit_depth[d] = 1 << (max_depth - d - 1);
      child_bit_depth_inv[d] = ~child_bit_depth[d];
    }
    
    mult = 1.0/real_voxelSize;
    add[0] = -center[0] + size;
    add[1] = -center[1] + size;
    add[2] = -center[2] + size;
    
    largest_index = child_bit_depth[0] * 2 -1;
  }
  
protected:
  
  ip::offset_ptr<bitoct> root;
  ip::offset_ptr<bitunion<T> > uroot;

  T center[3];

  T size;

  T voxelSize;

  T real_voxelSize;

  T add[3];
  T mult;

  unsigned int POINTDIM;

  ip::offset_ptr<T> mins;
  ip::offset_ptr<T> maxs;

  PointType pointtype;
  
  unsigned char max_depth;
  ip::offset_ptr<unsigned int> child_bit_depth;
  ip::offset_ptr<unsigned int> child_bit_depth_inv;
  int largest_index;

  static NNParams params[100];
  
  bool earlystop;

  Allocator* alloc;
  
public:
  
  inline const T* getMins() const { return &(*mins); }
  inline const T* getMaxs() const { return &(*maxs); }
  inline const T* getCenter() const { return center; }
  inline T getSize() const { return size; }
  inline unsigned int getPointdim() const { return POINTDIM; }
  inline const bitoct& getRoot() const { return *root; }
  inline unsigned int getMaxDepth() const { return max_depth; }
  
  inline void getCenter(double _center[3]) const {
    _center[0] = center[0];
    _center[1] = center[1];
    _center[2] = center[2];
  }

  void GetOctTreeCenter(vector<T*>&c) { GetOctTreeCenter(c, *root, center, size); }
  void GetOctTreeRandom(vector<T*>&c) { GetOctTreeRandom(c, *root); }
  void GetOctTreeRandom(vector<T*>&c, unsigned int ptspervoxel) { GetOctTreeRandom(c, ptspervoxel, *root); }
  void AllPoints(vector<T *> &vp) { AllPoints(*BOctTree<T>::root, vp); }

  long countNodes() { return 1 + countNodes(*root); } // computes number of inner nodes
  long countLeaves() { return countLeaves(*root); }   // computes number of leaves + points
  long countOctLeaves() { return countOctLeaves(*root); } // computes number of leaves

  void deserialize(std::string filename ) {
    char buffer[sizeof(T) * 20];
    T *p = reinterpret_cast<T*>(buffer);

    std::ifstream file;
    file.open (filename.c_str(), std::ios::in | std::ios::binary);

    // read magic bits
    file.read(buffer, 2);
    if ( buffer[0] != 'X' || buffer[1] != 'T') {
      std::cerr << "Not an octree file!!" << endl;
      file.close();
      return;
    }

    // read header
    pointtype = PointType::deserialize(file);

    file.read(buffer, 5 * sizeof(T));
    voxelSize = p[0];
    center[0] = p[1];
    center[1] = p[2];
    center[2] = p[3];
    size = p[4];

    file.read(buffer, sizeof(int));
    int *ip = reinterpret_cast<int*>(buffer);
    POINTDIM = *ip;

    mins = alloc->allocate<T>(POINTDIM);
    maxs = alloc->allocate<T>(POINTDIM);

    file.read(reinterpret_cast<char*>(&(*mins)), POINTDIM * sizeof(T));
    file.read(reinterpret_cast<char*>(&(*maxs)), POINTDIM * sizeof(T));

    // read root node
    uroot = alloc->allocate<bitunion<T> >();    
    root = &uroot->node;
    
    deserialize(file, *root);
    file.close();
  }
  
  static void deserialize(std::string filename, vector<Point> &points ) {
    char buffer[sizeof(T) * 20];

    std::ifstream file;
    file.open (filename.c_str(), std::ios::in | std::ios::binary);

    // read magic bits
    file.read(buffer, 2);
    if ( buffer[0] != 'X' || buffer[1] != 'T') {
      std::cerr << "Not an octree file!!" << endl;
      file.close();
      return;
    }

    // read header
    PointType pointtype = PointType::deserialize(file);

    file.read(buffer, 5 * sizeof(T)); // read over voxelsize, center and size
    file.read(buffer, sizeof(int));

    int *ip = reinterpret_cast<int*>(buffer);
    unsigned int POINTDIM = *ip;

    file.read(buffer, POINTDIM * sizeof(T));
    file.read(buffer, POINTDIM * sizeof(T));

    // read root node
    deserialize(file, points, pointtype);
    file.close();
  }

  void serialize(std::string filename) {
    char buffer[sizeof(T) * 20];
    T *p = reinterpret_cast<T*>(buffer);

    std::ofstream file;
    file.open (filename.c_str(), std::ios::out | std::ios::binary);

    // write magic bits
    buffer[0] = 'X';
    buffer[1] = 'T';
    file.write(buffer, 2);

    // write header
    pointtype.serialize(file);

    p[0] = voxelSize;
    p[1] = center[0]; 
    p[2] = center[1]; 
    p[3] = center[2];
    p[4] = size;

    int *ip = reinterpret_cast<int*>(&(buffer[5 * sizeof(T)]));
    *ip = POINTDIM;

    file.write(buffer, 5 * sizeof(T) + sizeof(int));


    for (unsigned int i = 0; i < POINTDIM; i++) {
      p[i] = mins[i];
    }
    for (unsigned int i = 0; i < POINTDIM; i++) {
      p[i+POINTDIM] = maxs[i];
    }

    file.write(buffer, 2*POINTDIM * sizeof(T));

    // write root node
    serialize(file, *root);

    file.close();
  }

  static PointType readType(std::string filename ) {
    char buffer[sizeof(T) * 20];

    std::ifstream file;
    file.open (filename.c_str(), std::ios::in | std::ios::binary);

    // read magic bits
    file.read(buffer, 2);
    if ( buffer[0] != 'X' || buffer[1] != 'T') {
      std::cerr << "Not an octree file!!" << endl;
      file.close();
      return PointType();
    }

    // read header
    PointType pointtype = PointType::deserialize(file);

    file.close();

    return pointtype;
  }
  

  T* pickPoint(bitoct &node) {
    bitunion<T> *children;
    bitoct::getChildren(node, children);

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
          // absolute pointer
          //return &(children->points[1].v);
          // offset pointer
          return children->getPoints();
        } else { // recurse
          return pickPoint(children->node);
        }
        ++children; // next child
      }
    }
    return 0;
  } 

  static void childcenter(const T *pcenter, T *ccenter, T size, unsigned char i) {
    switch (i) {
      case 0:  // 000
        ccenter[0] = pcenter[0] - size / 2.0;
        ccenter[1] = pcenter[1] - size / 2.0;
        ccenter[2] = pcenter[2] - size / 2.0;
        break;
      case 1:  // 001
        ccenter[0] = pcenter[0] + size / 2.0;
        ccenter[1] = pcenter[1] - size / 2.0;
        ccenter[2] = pcenter[2] - size / 2.0;
        break;
      case 2:  // 010
        ccenter[0] = pcenter[0] - size / 2.0;
        ccenter[1] = pcenter[1] + size / 2.0;
        ccenter[2] = pcenter[2] - size / 2.0;
        break;
      case 3:  // 011
        ccenter[0] = pcenter[0] + size / 2.0;
        ccenter[1] = pcenter[1] + size / 2.0;
        ccenter[2] = pcenter[2] - size / 2.0;
        break;
      case 4:  // 100
        ccenter[0] = pcenter[0] - size / 2.0;
        ccenter[1] = pcenter[1] - size / 2.0;
        ccenter[2] = pcenter[2] + size / 2.0;
        break;
      case 5:  // 101
        ccenter[0] = pcenter[0] + size / 2.0;
        ccenter[1] = pcenter[1] - size / 2.0;
        ccenter[2] = pcenter[2] + size / 2.0;
        break;
      case 6:  // 110
        ccenter[0] = pcenter[0] - size / 2.0;
        ccenter[1] = pcenter[1] + size / 2.0;
        ccenter[2] = pcenter[2] + size / 2.0;
        break;
      case 7:  // 111
        ccenter[0] = pcenter[0] + size / 2.0;
        ccenter[1] = pcenter[1] + size / 2.0;
        ccenter[2] = pcenter[2] + size / 2.0;
        break;
      default:
        break;
    }
  }
  
  static void childcenter(int x, int y, int z, int &cx, int &cy, int &cz, char i, int size) {
    switch (i) {
      case 0:  // 000
        cx = x - size ;
        cy = y - size ;
        cz = z - size ;
        break;
      case 1:  // 001
        cx = x + size ;
        cy = y - size ;
        cz = z - size ;
        break;
      case 2:  // 010
        cx = x - size ;
        cy = y + size ;
        cz = z - size ;
        break;
      case 3:  // 011
        cx = x + size ;
        cy = y + size ;
        cz = z - size ;
        break;
      case 4:  // 100
        cx = x - size ;
        cy = y - size ;
        cz = z + size ;
        break;
      case 5:  // 101
        cx = x + size ;
        cy = y - size ;
        cz = z + size ;
        break;
      case 6:  // 110
        cx = x - size ;
        cy = y + size ;
        cz = z + size ;
        break;
      case 7:  // 111
        cx = x + size ;
        cy = y + size ;
        cz = z + size ;
        break;
      default:
        break;
    }
  }
  
protected:
  
  void AllPoints( bitoct &node, vector<T*> &vp) {
    bitunion<T> *children;
    bitoct::getChildren(node, children);

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
          // absolute pointer
          //pointrep *points = children->points;
          // offset pointer
          pointrep* points = children->getPointreps();
          unsigned int length = points[0].length;
          T *point = &(points[1].v);  // first point
          for(unsigned int iterator = 0; iterator < length; iterator++ ) {
            //T *p = new T[BOctTree<T>::POINTDIM];
//            T *p = new T[3];
//            p[0] = point[0]; p[1] = point[1]; p[2] = point[2];
            T *p = new T[BOctTree<T>::POINTDIM];
            for (unsigned int k = 0; k < BOctTree<T>::POINTDIM; k++)
              p[k] = point[k];

            vp.push_back(p);

            //glVertex3f( point[0], point[1], point[2]);
            point+=BOctTree<T>::POINTDIM;
          }
        } else { // recurse
          AllPoints( children->node, vp);
        }
        ++children; // next child
      }
    }
  }
  
  static void deserialize(std::ifstream &f, vector<Point> &vpoints, PointType &pointtype) {
    char buffer[2];
    pointrep *point = new pointrep[pointtype.getPointDim()];
    f.read(buffer, 2);
    bitoct node;
    node.valid = buffer[0];
    node.leaf = buffer[1];

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf read points 
          pointrep first;
          f.read(reinterpret_cast<char*>(&first), sizeof(pointrep));
          unsigned int length = first.length;  // read first element, which is the length
          for (unsigned int k = 0; k < length; k++) {
            f.read(reinterpret_cast<char*>(point), sizeof(pointrep) * pointtype.getPointDim()); // read the points
            vpoints.push_back( pointtype.createPoint( &(point->v ) ) );
          }
        } else {  // write child 
          deserialize(f, vpoints, pointtype);
        }
      }
    }
    delete [] point;
  }

  void deserialize(std::ifstream &f, bitoct &node) {
    char buffer[2];
    f.read(buffer, 2);
    node.valid = buffer[0];
    node.leaf = buffer[1];

    unsigned short n_children = POPCOUNT(node.valid);

    // create children
    bitunion<T> *children = alloc->allocate<bitunion<T> >(n_children);    
    bitoct::link(node, children);

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf read points 
          pointrep first;
          f.read(reinterpret_cast<char*>(&first), sizeof(pointrep));
          unsigned int length = first.length;  // read first element, which is the length
          pointrep *points = alloc->allocate<pointrep> (POINTDIM*length + 1);
          // absolute pointer
          //children->points = points;
          // offset pointer
          bitunion<T>::link(children, points);
          points[0] = first;
          points++;
          f.read(reinterpret_cast<char*>(points), sizeof(pointrep) * length * POINTDIM); // read the points
        } else {  // write child 
          deserialize(f, children->node);
        }
        ++children; // next child
      }
    }
  }

  void serialize(std::ofstream &of, bitoct &node) {
    char buffer[2];
    buffer[0] = node.valid;
    buffer[1] = node.leaf;
    of.write(buffer, 2);


    // write children
    bitunion<T> *children;
    bitoct::getChildren(node, children);
    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf write points
          // absolute pointer
          //pointrep *points = children->points;
          // offset pointer
          pointrep* points = children->getPointreps();
          unsigned int length = points[0].length;
          of.write(reinterpret_cast<char*>(points), sizeof(pointrep) * (length * POINTDIM  +1));
        } else {  // write child 
          serialize(of, children->node);
        }
        ++children; // next child
      }
    }
  }
  
  void GetOctTreeCenter(vector<T*>&c, bitoct &node, T *center, T size) {
    T ccenter[3];
    bitunion<T> *children;
    bitoct::getChildren(node, children);

    for (unsigned char i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        childcenter(center, ccenter, size, i);  // childrens center
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf get center
          T * cp = new T[3];
          for (unsigned int iterator = 0; iterator < 3; iterator++) {
            cp[iterator] = ccenter[iterator];
          }
          c.push_back(cp);
        } else { // recurse
          GetOctTreeCenter(c, children->node, ccenter, size/2.0);
        }
        ++children; // next child
      }
    }
  }

  void GetOctTreeRandom(vector<T*>&c, bitoct &node) {
    bitunion<T> *children;
    bitoct::getChildren(node, children);

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
          // absolute pointer
          //pointrep *points = children->points;
          // offset pointer
          pointrep* points = children->getPointreps();
          // new version to ignore leaves with less than 3 points
          /* 
          if(points[0].length > 2) { 
            for(int tmp = 0; tmp < points[0].length; tmp++) {
              T *point = &(points[POINTDIM*tmp+1].v);
              c.push_back(point);
            }
          }
          */  
          //old version
          
          int tmp = rand(points[0].length);
          T *point = &(points[POINTDIM*tmp+1].v);
          c.push_back(point);
          

        } else { // recurse
          GetOctTreeRandom(c, children->node);
        }
        ++children; // next child
      }
    }
  } 
  
  void GetOctTreeRandom(vector<T*>&c, unsigned int ptspervoxel, bitoct &node) {
    bitunion<T> *children;
    bitoct::getChildren(node, children);

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
          // absolute pointer
          //pointrep *points = children->points;
          // offset pointer
          pointrep* points = children->getPointreps();
          unsigned int length = points[0].length;
          if (ptspervoxel >= length) {
            for (unsigned int j = 0; j < length; j++) 
              c.push_back(&(points[POINTDIM*j+1].v));

            ++children; // next child
            continue;
          }
          set<int> indices;
          while(indices.size() < ptspervoxel) {
            int tmp = rand(length-1);
            indices.insert(tmp);
          }
          for(set<int>::iterator it = indices.begin(); it != indices.end(); it++) 
            c.push_back(&(points[POINTDIM*(*it)+1].v));

        } else { // recurse
          GetOctTreeRandom(c, ptspervoxel, children->node);
        }
        ++children; // next child
      }
    }
  }

  long countNodes(bitoct &node) {
    long result = 0;
    bitunion<T> *children;
    bitoct::getChildren(node, children);

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
          //++result;
        } else { // recurse
          result += countNodes(children->node) + 1;
        }
        ++children; // next child
      }
    }
    return result;
  }

  long countLeaves(bitoct &node) {
    long result = 0;
    bitunion<T> *children;
    bitoct::getChildren(node, children);

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
          long nrpts = children->getLength();
          result += POINTDIM*nrpts;
        } else { // recurse
          result += countLeaves(children->node);
        }
        ++children; // next child
      }
    }
    return result;
  }
  
  long countOctLeaves(bitoct &node) {
    long result = 0;
    bitunion<T> *children;
    bitoct::getChildren(node, children);

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
          result ++;
        } else { // recurse
          result += countTrueLeaves(children->node);
        }
        ++children; // next child
      }
    }
    return result;
  }

  // TODO: is this still needed? nodes and pointreps are all in the Allocator
  void deletetNodes(bitoct &node) {
    bitunion<T> *children;
    bitoct::getChildren(node, children);
    bool haschildren = false;

    for (short i = 0; i < 8; i++) {
      if (  ( 1 << i ) & node.valid ) {   // if ith node exists
        if (  ( 1 << i ) & node.leaf ) {   // if ith node is leaf
          // absolute pointer
          //delete [] children->points;
          // offset pointer
          delete [] children->getPointreps();
        } else { // recurse
          deletetNodes(children->node);
        }
        ++children; // next child
        haschildren = true;
      }
    }
    // delete children
    if (haschildren) {
      bitoct::getChildren(node, children);
    }
  }
  
  template <class P>
  void* branch( bitoct &node, P * const * splitPoints, int n,  T _center[3], T _size) {
    // if bucket is too small stop building tree
    // -----------------------------------------
    if ((_size <= voxelSize) || (earlystop && n <= 10) ) {

      // copy points
      pointrep *points = alloc->allocate<pointrep> (POINTDIM*n + 1);

      points[0].length = n;
      int i = 1;
      for (int j = 0; j < n; j++) {
        for (unsigned int iterator = 0; iterator < POINTDIM; iterator++) {
          points[i++].v = splitPoints[j][iterator];
        }
      }
      return points; 
    }  

    // calculate new buckets
    T newcenter[8][3];
    T sizeNew;

    sizeNew = _size / 2.0;

    for (unsigned char i = 0; i < 8; i++) {
      childcenter(_center, newcenter[i], _size, i);
    }

    countPointsAndQueueFast(splitPoints, n, newcenter, sizeNew, node, _center);
    return 0;
  }

  template <class P>
  void* branch( bitoct &node, vector<P*> &splitPoints, T _center[3], T _size) {
    // if bucket is too small stop building tree
    // -----------------------------------------
    if ((_size <= voxelSize) || (earlystop && splitPoints.size() <= 10) ) {
      // copy points
      pointrep *points = alloc->allocate<pointrep> (POINTDIM*splitPoints.size() + 1);
      points[0].length = splitPoints.size();
      int i = 1;
      for (typename vector<P *>::iterator itr = splitPoints.begin(); 
          itr != splitPoints.end(); itr++) {
        for (unsigned int iterator = 0; iterator < POINTDIM; iterator++) {
          points[i++].v = (*itr)[iterator];
        }
      }
      return points; 
    }  

    // calculate new buckets
    T newcenter[8][3];
    T sizeNew;

    sizeNew = _size / 2.0;

    for (unsigned char i = 0; i < 8; i++) {
      childcenter(_center, newcenter[i], _size, i);
    }

    countPointsAndQueue(splitPoints, newcenter, sizeNew, node, _center);
    return 0;
  }

  template <class P>
  void countPointsAndQueue(vector<P*> &i_points, T center[8][3], T size, bitoct &parent, T *pcenter) {
    vector<P*> points[8];
    int n_children = 0;
    for (typename vector<P *>::iterator itr = i_points.begin(); itr != i_points.end(); itr++) {
      points[childIndex<P>(pcenter, *itr)].push_back( *itr );
    }

    i_points.clear();
    vector<P*>().swap(i_points);
    for (int j = 0; j < 8; j++) {
      if (!points[j].empty()) {
        parent.valid = ( 1 << j ) | parent.valid;
        ++n_children;
      }
    }
    // create children
    bitunion<T> *children = alloc->allocate<bitunion<T> >(n_children);
    bitoct::link(parent, children);

    int count = 0;
    for (int j = 0; j < 8; j++) {
      if (!points[j].empty()) {
        pointrep *c = (pointrep*)branch(children[count].node, points[j], center[j], size);  // leaf node
        if (c) {
          // absolute pointer
          //children[count].points = c; // set this child to deque of points
          // offset pointer
          bitunion<T>::link(&children[count], c);
          parent.leaf = ( 1 << j ) | parent.leaf;  // remember this is a leaf
        }
        points[j].clear();
        vector<P*>().swap(points[j]);
        ++count;
      }
    }
  }
  
  template <class P>
  void countPointsAndQueueFast(P * const* points, int n,  T center[8][3], T size, bitoct &parent, T pcenter[3]) {
    P * const *blocks[9];
    blocks[0] = points;
    blocks[8] = points + n;
    fullsort(points, n, pcenter, blocks+1);

    int n_children = 0;
    
    for (int j = 0; j < 8; j++) {
      // if non-empty set valid flag for this child
      if (blocks[j+1] - blocks[j] > 0) {
        parent.valid = ( 1 << j ) | parent.valid;
        ++n_children;
      }
    }

    // create children
    bitunion<T> *children = alloc->allocate<bitunion<T> >(n_children);
    bitoct::link(parent, children);
    int count = 0;
    for (int j = 0; j < 8; j++) {
      if (blocks[j+1] - blocks[j] > 0) {
        pointrep *c = (pointrep*)branch(children[count].node, blocks[j], blocks[j+1] - blocks[j], center[j], size);  // leaf node
        if (c) {
          // absolute pointer
          //children[count].points = c; // set this child to vector of points
          // offset pointer
          bitunion<T>::link(&children[count], c); // set this child to vector of points
          parent.leaf = ( 1 << j ) | parent.leaf;  // remember this is a leaf
        }
        ++count;
      }
    }
  }


  void getByIndex(T *point, T *&points, unsigned int &length) {
    unsigned int x,y,z;
    x = (point[0] + add[0]) * mult;
    y = (point[1] + add[1]) * mult;
    z = (point[2] + add[2]) * mult;
    
    bitunion<T> *node = uroot; 
    unsigned char child_index;
    unsigned int child_bit;
    unsigned int depth = 0;

    while (true) {
      child_bit = child_bit_depth[depth];
      child_index = ((x & child_bit )!=0)  | (((y & child_bit )!=0 )<< 1) | (((z & child_bit )!=0) << 2);
      if (node->childIsLeaf(child_index) ) {
        node = node->getChild(child_index);
        points = node->getPoints();
        length = node->getLength();
        return;
      } else {
        node = node->getChild(child_index);
      }
      depth++;
    }
  }

  template <class P>
  inline unsigned char childIndex(const T *center, const P *point) {
    return  (point[0] > center[0] ) | ((point[1] > center[1] ) << 1) | ((point[2] > center[2] ) << 2) ;
  }

  inline void findClosestInLeaf(bitunion<T> *node, int threadNum) const {
    if (params[threadNum].count >= params[threadNum].max_count) return;
    params[threadNum].count++;
    T* points = node->getPoints();
    unsigned int length = node->getLength();
    for(unsigned int iterator = 0; iterator < length; iterator++ ) {
      double myd2 = Dist2(params[threadNum].p, points); 
      if (myd2 < params[threadNum].closest_d2) {
        params[threadNum].closest_d2 = myd2;
        params[threadNum].closest = points;
        if (myd2 <= 0.0001) {
          params[threadNum].closest_v = 0; // the search radius in units of voxelSize
        } else {
          params[threadNum].closest_v = sqrt(myd2) * mult + 1; // the search radius in units of voxelSize
        }
      }
      points+=BOctTree<T>::POINTDIM;
    }
  }
  


  double *FindClosest(double *point, double maxdist2, int threadNum) const
  {
    params[threadNum].closest = 0; // no point found currently
    params[threadNum].closest_d2 = maxdist2;
    params[threadNum].p = point;
    params[threadNum].x = (point[0] + add[0]) * mult;
    params[threadNum].y = (point[1] + add[1]) * mult;
    params[threadNum].z = (point[2] + add[2]) * mult;
    params[threadNum].closest_v = sqrt(maxdist2) * mult + 1; // the search radius in units of voxelSize
    params[threadNum].count = 0;
    params[threadNum].max_count = 10000; // stop looking after this many buckets

   
    // box within bounds in voxel coordinates
    int xmin, ymin, zmin, xmax, ymax, zmax;
    xmin = max(params[threadNum].x-params[threadNum].closest_v, 0); 
    ymin = max(params[threadNum].y-params[threadNum].closest_v, 0); 
    zmin = max(params[threadNum].z-params[threadNum].closest_v, 0);

//    int largest_index = child_bit_depth[0] * 2 -1;
    
    xmax = min(params[threadNum].x+params[threadNum].closest_v, largest_index);
    ymax = min(params[threadNum].y+params[threadNum].closest_v, largest_index);
    zmax = min(params[threadNum].z+params[threadNum].closest_v, largest_index);
    
    unsigned char depth = 0;
    unsigned int child_bit;
    unsigned int child_index_min;
    unsigned int child_index_max;

    bitunion<T> *node = &(*uroot);

    int cx, cy, cz;
    
    child_bit = child_bit_depth[depth];
    cx = child_bit_depth[depth];
    cy = child_bit_depth[depth];
    cz = child_bit_depth[depth];

    while (true) { // find the first node where branching is required
      child_index_min = ((xmin & child_bit )!=0)  | (((ymin & child_bit )!=0 )<< 1) | (((zmin & child_bit )!=0) << 2);
      child_index_max = ((xmax & child_bit )!=0)  | (((ymax & child_bit )!=0 )<< 1) | (((zmax & child_bit )!=0) << 2);

      // if these are the same, go there
      // TODO: optimization: also traverse if only single child...
      if (child_index_min == child_index_max) {
        if (node->childIsLeaf(child_index_min) ) {  // luckily, no branching is required
          findClosestInLeaf(node->getChild(child_index_min), threadNum);
          return static_cast<double*>(params[threadNum].closest);
        } else {
          if (node->isValid(child_index_min) ) { // only descend when there is a child
            childcenter(cx,cy,cz, cx,cy,cz, child_index_min, child_bit/2 ); 
            node = node->getChild(child_index_min);
            child_bit /= 2;
          } else {  // there is no child containing the bounding box => no point is close enough
            return 0;
          }
        }
      } else {
        // if min and max are not in the same child we must branch
        break;
      }
    }
    
    // node contains all box-within-bounds cells, now begin best bin first search
    _FindClosest(threadNum, node->node, child_bit/2, cx, cy, cz);
    return static_cast<double*>(params[threadNum].closest);
  }
  
  void _FindClosest(int threadNum, bitoct &node, int size, int x, int y, int z) const
  {
    // Recursive case
   
    // compute which child is closest to the query point
    unsigned char child_index =  ((params[threadNum].x - x) >= 0) | 
                                (((params[threadNum].y - y) >= 0) << 1) | 
                                (((params[threadNum].z - z) >= 0) << 2);
    
    char *seq2ci = sequence2ci[child_index][node.valid];  // maps preference to index in children array
    char *mmap = amap[child_index];  // maps preference to area index 

    bitunion<T> *children;
    bitoct::getChildren(node, children);
    int cx, cy, cz;
    cx = cy = cz = 0; // just to shut up the compiler warnings
    for (unsigned char i = 0; i < 8; i++) { // in order of preference
      child_index = mmap[i]; // the area index of the node 
      if (  ( 1 << child_index ) & node.valid ) {   // if ith node exists
        childcenter(x,y,z, cx,cy,cz, child_index, size); 
        if ( params[threadNum].closest_v == 0 ||  max(max(abs( cx - params[threadNum].x ), 
                 abs( cy - params[threadNum].y )),
                 abs( cz - params[threadNum].z )) - size
        > params[threadNum].closest_v ) { 
          continue;
        }
        // find the closest point in leaf seq2ci[i] 
        if (  ( 1 << child_index ) & node.leaf ) {   // if ith node is leaf
          findClosestInLeaf( &children[seq2ci[i]], threadNum);
        } else { // recurse
          _FindClosest(threadNum, children[seq2ci[i]].node, size/2, cx, cy, cz);
        }
      }
    }
  }


  double *FindClosestInBucket(double *point, double maxdist2, int threadNum) {
    params[threadNum].closest = 0;
    params[threadNum].closest_d2 = maxdist2;
    params[threadNum].p = point;
    unsigned int x,y,z;
    x = (point[0] + add[0]) * mult;
    y = (point[1] + add[1]) * mult;
    z = (point[2] + add[2]) * mult;
    T * points;
    unsigned int length;

    bitunion<T> *node = uroot; 
    unsigned char child_index;

    unsigned int  child_bit = child_bit_depth[0];

    while (true) {
      child_index = ((x & child_bit )!=0)  | (((y & child_bit )!=0 )<< 1) | (((z & child_bit )!=0) << 2);
      if (node->childIsLeaf(child_index) ) {
        node = node->getChild(child_index);
        points = node->getPoints();
        length = node->getLength();
        
        for(unsigned int iterator = 0; iterator < length; iterator++ ) {
          double myd2 = Dist2(params[threadNum].p, points); 
          if (myd2 < params[threadNum].closest_d2) {
            params[threadNum].closest_d2 = myd2;
            params[threadNum].closest = points;
          }
          points+=BOctTree<T>::POINTDIM;
        }
        return static_cast<double*>(params[threadNum].closest);
      } else {
        if (node->isValid(child_index) ) {
          node = node->getChild(child_index);
        } else {
          return 0;
        }
      }
      child_bit >>= 1;
    }
    return static_cast<double*>(params[threadNum].closest);
  }
  

template <class P>
  void fullsort(P * const * points, int n, T splitval[3], P * const * blocks[9]) {
    P* const * L0;
    P* const * L1;
    P* const * L2;
    unsigned int n0L, n0R, n1L, n1R ;

    // sort along Z
    L0 = sort(points, n, splitval[2], 2);
      
      n0L = L0 - points;
      // sort along Y (left of Z)   points -- L0
      L1 = sort(points, n0L, splitval[1], 1);

        n1L = L1 - points; 
        // sort along X (left of Y)  points -- L1
        L2 = sort(points, n1L, splitval[0], 0);
        
          blocks[0] = L2;
        
        n1R = n0L - n1L;
        // sort along X (right of Y) // L1 -- L0
        L2 = sort(L1, n1R, splitval[0], 0);
        
          blocks[1] = L1;
          blocks[2] = L2;

      n0R = n - n0L;
      // sort along Y (right of Z)  L0 -- end
      L1 = sort(L0, n0R, splitval[1], 1);
      
        n1L = L1 - L0; 
        // sort along X (left of Y)  points -- L1
        L2 = sort(L0, n1L, splitval[0], 0);
        
          blocks[3] = L0;
          blocks[4] = L2;
        
        n1R = n0R - n1L;
        // sort along X (right of Y) // L1 -- L0
        L2 = sort(L1, n1R, splitval[0], 0);
        
          blocks[5] = L1;
          blocks[6] = L2;
  }


  template <class P>
  P* const * sort(P* const * points, unsigned int n, T splitval, unsigned char index) {
    if (n==0) return points;
    
    if (n==1) {
      if (points[0][index] < splitval)
        return points+1;
      else
        return points;
    }

    P **left = const_cast<P**>(points);
    P **right = const_cast<P**>(points + n - 1);
    
    
      while (1) {
      while ((*left)[index] < splitval) 
      {
        left++;
        if (right < left)
          break;
      }
      while ((*right)[index] >= splitval)
      {
        right--;
        if (right < left)
          break;
      }
      if (right < left)
        break;

      std::swap(*left, *right);
    }
    return left;
  }

public:
  BOctTree(const BOctTree& other, unsigned char* mem_ptr, unsigned int mem_max)
  {
    alloc = new SequentialAllocator(mem_ptr, mem_max);
    
    // "allocate" space for *this
    alloc->allocate<BOctTree<T> >();
    
    // take members
    unsigned int i;
    for(i = 0; i < 3; ++i)
      center[i] = other.center[i];
    size = other.size;
    voxelSize = other.voxelSize;
    real_voxelSize = other.real_voxelSize;
    for(i = 0; i < 3; ++i)
      add[i] = other.add[i];
    mult = other.mult;
    POINTDIM = other.POINTDIM;
    mins = alloc->allocate<T>(POINTDIM);
    maxs = alloc->allocate<T>(POINTDIM);
    for(i = 0; i < POINTDIM; ++i) {
      mins[i] = other.mins[i];
      maxs[i] = other.maxs[i];
    }
    pointtype = other.pointtype;
    max_depth = other.max_depth;
    child_bit_depth = alloc->allocate<unsigned int>(max_depth);
    child_bit_depth_inv = alloc->allocate<unsigned int>(max_depth);
    for(i = 0; i < max_depth; ++i) {
      child_bit_depth[i] = other.child_bit_depth[i];
      child_bit_depth_inv[i] = other.child_bit_depth_inv[i];
    }
    largest_index = other.largest_index;
    
    // take node structure
    uroot = alloc->allocate<bitunion<T> >();  
    root = &uroot->node;
    copy_children(*other.root, *root);
    
    // test if allocator has used up his space
    //alloc->printSize();
    
    // discard allocator, space is managed by the cache manager
    delete alloc; alloc = 0;
  }

private:
  void copy_children(const bitoct& other, bitoct& my) {
    // copy node attributes
    my.valid = other.valid;
    my.leaf = other.leaf;
    
    // other children
    bitunion<T>* other_children;
    bitoct::getChildren(other, other_children);
    
    // create own children
    unsigned int n_children = POPCOUNT(other.valid);
    bitunion<T>* my_children = alloc->allocate<bitunion<T> >(n_children);    
    bitoct::link(my, my_children);
    
    // iterate over all (valid) children and copy them
    for(unsigned int i = 0; i < 8; ++i) {
      if((1<<i) & other.valid) {
        if((1<<i) & other.leaf) {
          // copy points
          unsigned int length = other_children->getLength();
          pointrep* other_pointreps = other_children->getPointreps();
          pointrep* my_pointreps = alloc->allocate<pointrep>(POINTDIM * length + 1);
          for(unsigned int j = 0; j < POINTDIM * length + 1; ++j)
            my_pointreps[j] = other_pointreps[j];
          // assign
          bitunion<T>::link(my_children, my_pointreps);
        } else {
          // child is already created, copy and create children for it
          copy_children(other_children->node, my_children->node);
        }
        ++other_children;
        ++my_children;
      }
    }
  }

public:
  unsigned int getMemorySize()
  {
    return sizeof(*this) // all member variables
      + 2*POINTDIM*sizeof(T) // mins, maxs
      + 2*max_depth*sizeof(unsigned int) // child_bit_depth(_inv)
      + sizeof(bitunion<T>) // uroot
      + sizeChildren(*root); // all nodes
  }
  
private:
  unsigned int sizeChildren(const bitoct& node) {
    unsigned int s = 0;
    bitunion<T>* children;
    bitoct::getChildren(node, children);
    
    // size of children allocation
    unsigned int n_children = POPCOUNT(node.valid);
    s += sizeof(bitunion<T>)*n_children;
    
    // iterate over all (valid) children and sum them up
    for(unsigned int i = 0; i < 8; ++i) {
      if((1<<i) & node.valid) {
        if((1<<i) & node.leaf) {
          // leaf only accounts for its points
          s += sizeof(pointrep)*(children->getLength()*POINTDIM+1);
        } else {
          // childe node is already accounted for, add its children
          s += sizeChildren(children->node);
        }
        ++children; // next (valid) child
      }
    }
    return s;
  }
};

typedef SingleObject<BOctTree<float> > DataOcttree;

template <class T>
NNParams BOctTree<T>::params[100];

#endif