3dpcp/.svn/pristine/6a/6a723ffc9b749b451dcd95c95db713042a156e25.svn-base

849 lines
28 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* compacttree implementation
*
* Copyright (C) Jan Elseberg, Kai Lingemann, Jan Elseberg
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Efficient representation of an octree
* @author Jan Elsberg. Automation Group, Jacobs University Bremen gGmbH, Germany.
* @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
* @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
*/
#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>
#include "slam6d/globals.icc"
#include "slam6d/point_type.h"
#include "slam6d/Boctree.h"
#include "show/compacttree.h"
#include "show/colormanager.h"
#include "show/scancolormanager.h"
#include "show/viewcull.h"
compactTree::~compactTree(){
delete alloc;
delete[] mins;
delete[] maxs;
}
void compactTree::AllPoints( cbitoct &node, vector<double*> &vp, double center[3], double size) {
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
for (short 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
tshort *point = children->getPoints();
lint length = children->getLength();
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
double *p = new double[3];
//cout << point[0] << " " << point[1] << " " << point[2] << endl;
for (unsigned int k = 0; k < 3; k++){
p[k] = point[k] * precision + ccenter[k];
}
vp.push_back(p);
point+=POINTDIM;
}
} else { // recurse
AllPoints( children->node, vp, ccenter, size/2.0);
}
++children; // next child
}
}
}
void compactTree::GetOctTreeCenter(vector<double*>&c, cbitoct &node, double *center, double size) {
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::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
double * cp = new double[POINTDIM];
for (unsigned int iterator = 0; iterator < POINTDIM; iterator++) {
cp[iterator] = ccenter[iterator];
}
c.push_back(cp);
} else { // recurse
GetOctTreeCenter(c, children->node, ccenter, size/2.0);
}
++children; // next child
}
}
}
long compactTree::countNodes(cbitoct &node) {
long result = 0;
cbitunion<tshort> *children;
cbitoct::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 compactTree::countLeaves(cbitoct &node) {
long result = 0;
cbitunion<tshort> *children;
cbitoct::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
lint nrpts = children->getLength();
result += POINTDIM*nrpts + 1;
} else { // recurse
result += countLeaves(children->node);
}
++children; // next child
}
}
return result;
}
void compactTree::deletetNodes(cbitoct &node) {
cbitunion<tshort> *children;
cbitoct::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
tshort *points = children->getPoints();
delete [] points;
// delete [] children->points;
} else { // recurse
deletetNodes(children->node);
}
++children; // next child
haschildren = true;
}
}
// delete children
if (haschildren) {
cbitoct::getChildren(node, children);
delete[] children;
}
}
unsigned long compactTree::maxTargetPoints( cbitoct &node ) {
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
unsigned long max = 0;
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
lint length = children->getLength();
if (length > max) max = length;
} else { // recurse
unsigned long tp = maxTargetPoints( children->node);
if (tp > max) max = tp;
}
++children; // next child
}
}
return max*POPCOUNT(node.valid);
}
void compactTree::displayOctTreeAll( cbitoct &node, double *center, double size) {
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
for (short 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
tshort *point = children->getPoints();
lint length = children->getLength();
glBegin(GL_POINTS);
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
if(cm) cm->setColor(point);
//cout << "C " << point[1] << " " << cm << endl;
//glVertex3f( point[0], point[1], point[2]);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
point+=POINTDIM;
}
glEnd();
} else { // recurse
displayOctTreeAll( children->node, ccenter, size/2.0);
}
++children; // next child
}
}
}
void compactTree::displayOctTreeAllCulled( cbitoct &node, double *center, double size ) {
int res = CubeInFrustum2(center[0], center[1], center[2], size);
if (res==0) return; // culled do not continue with this branch of the tree
if (res == 2) { // if entirely within frustrum discontinue culling
displayOctTreeAll(node, center, size);
return;
}
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
for (short 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
// check if leaf is visible
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
tshort *point = children->getPoints();
lint length = children->getLength();
glBegin(GL_POINTS);
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
if(cm) cm->setColor(point);
//glVertex3f( point[0], point[1], point[2]);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
point+=POINTDIM;
}
glEnd();
}
} else { // recurse
displayOctTreeAllCulled( children->node, ccenter, size/2.0);
}
++children; // next child
}
}
}
void compactTree::displayOctTreeCulledLOD(long targetpts, cbitoct &node, double *center, double size ) {
if (targetpts <= 0) return; // no need to display anything
int res = CubeInFrustum2(center[0], center[1], center[2], size);
if (res==0) return; // culled do not continue with this branch of the tree
if (res == 2) { // if entirely within frustrum discontinue culling
displayOctTreeLOD(targetpts, node, center, size);
return;
}
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
unsigned short nc = POPCOUNT(node.valid);
long newtargetpts = targetpts;
if (nc > 0) {
newtargetpts = newtargetpts/nc;
if (newtargetpts <= 0 ) return;
}
for (short 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
// check if leaf is visible
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
tshort *point = children->getPoints();
lint length = children->getLength();
glBegin(GL_POINTS);
if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { // only a single pixel on screen only paint one point
if(cm) cm->setColor(point);
//glVertex3f( point[0], point[1], point[2]);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
} else if (length <= newtargetpts) { // more points requested than possible, plot all
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
if(cm) cm->setColor(point);
//glVertex3f( point[0], point[1], point[2]);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
point+=POINTDIM;
}
} else { // select points to show
// TODO smarter subselection of points here
double each = (double)POINTDIM * (double)((double)length/(double)newtargetpts);
tshort *p;
int index;
for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) {
index = (double)iterator * each;
p = point + index - index%POINTDIM;
if(cm) cm->setColor(p);
//glVertex3f( p[0], p[1], p[2]);
glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]);
//point += each;
}
}
glEnd();
}
} else { // recurse
displayOctTreeCulledLOD(newtargetpts, children->node, ccenter, size/2.0);
}
++children; // next child
}
}
}
void compactTree::displayOctTreeLOD(long targetpts, cbitoct &node, double *center, double size ) {
if (targetpts <= 0) return; // no need to display anything
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
unsigned short nc = POPCOUNT(node.valid);
long newtargetpts = targetpts;
if (nc > 0) {
newtargetpts = newtargetpts/nc;
if (newtargetpts <= 0 ) return;
}
for (short 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
tshort *point = children->getPoints();
lint length = children->getLength();
glBegin(GL_POINTS);
/* if (length > 10 && !LOD(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) { // only a single pixel on screen only paint one point
if(cm) cm->setColor(point);
//glVertex3f( point[0], point[1], point[2]);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
} else*/ if (length <= newtargetpts) { // more points requested than possible, plot all
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
if(cm) cm->setColor(point);
//glVertex3f( point[0], point[1], point[2]);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
point+=POINTDIM;
}
} else { // select points to show
// TODO smarter subselection of points here
double each = (double)POINTDIM * (double)((double)length/(double)newtargetpts);
tshort *p;
int index;
for(unsigned int iterator = 0; iterator < newtargetpts; iterator++ ) {
index = (double)iterator * each;
p = point + index - index%POINTDIM;
if(cm) cm->setColor(p);
//glVertex3f( p[0], p[1], p[2]);
glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]);
//point += each;
}
}
glEnd();
} else { // recurse
displayOctTreeLOD(newtargetpts, children->node, ccenter, size/2.0);
}
++children; // next child
}
}
}
void compactTree::displayOctTreeCulledLOD2(float ratio, cbitoct &node, double *center, double size ) {
int res = CubeInFrustum2(center[0], center[1], center[2], size);
if (res==0) return; // culled do not continue with this branch of the tree
if (res == 2) { // if entirely within frustrum discontinue culling
displayOctTreeLOD2(ratio, node, center, size);
return;
}
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
for (short 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
// check if leaf is visible
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
tshort *point = children->getPoints();
lint length = children->getLength();
int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
l = max((int)(l*l*ratio), 0);
if (l != 0) {
if ((int)length > l ) {
double each = (double)POINTDIM * (double)((double)length/(double)l);
tshort *p;
int index;
for(int iterator = 0; iterator < l; iterator++ ) {
index = (double)iterator * each;
p = point + index - index%POINTDIM;
if(cm) cm->setColor(p);
glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]);
}
} else if ((int)length <= l) {
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
if(cm) cm->setColor(point);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
point+=POINTDIM;
}
} else if (l == 1) {
if(cm) cm->setColor(point);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
}
}
}
} else { // recurse
int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
l = max((int)(l*l*ratio), 0);
if (l > 0) {
displayOctTreeCulledLOD2(ratio, children->node, ccenter, size/2.0);
}
}
++children; // next child
}
}
}
void compactTree::displayOctTreeLOD2(float ratio, cbitoct &node, double *center, double size ) {
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
for (short 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
tshort *point = children->getPoints();
lint length = children->getLength();
int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
l = max((int)(l*l*ratio), 0);
if (l > 1) {
if ((int)length > l ) {
double each = (double)POINTDIM * (double)((double)length/(double)l);
tshort *p;
int index;
for(int iterator = 0; iterator < l; iterator++ ) {
index = (double)iterator * each;
p = point + index - index%POINTDIM;
if(cm) cm->setColor(p);
glVertex3f( p[0] * precision + ccenter[0], p[1] * precision + ccenter[1], p[2] * precision + ccenter[2]);
}
} else if ((int)length <= l) {
for(unsigned int iterator = 0; iterator < length; iterator++ ) {
if(cm) cm->setColor(point);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
point+=POINTDIM;
}
}
} else {
if(cm) cm->setColor(point);
glVertex3f( point[0] * precision + ccenter[0], point[1] * precision + ccenter[1], point[2] * precision + ccenter[2]);
}
} else { // recurse
int l = LOD2(ccenter[0], ccenter[1], ccenter[2], size/2.0); // only a single pixel on screen only paint one point
l = max((int)(l*l*ratio), 0);
if (l > 0) {
displayOctTreeLOD2(ratio, children->node, ccenter, size/2.0);
}
}
++children; // next child
}
}
}
void compactTree::displayOctTreeCAllCulled( cbitoct &node, double *center, double size, double minsize ) {
int res = CubeInFrustum2(center[0], center[1], center[2], size);
if (res==0) return; // culled do not continue with this branch of the tree
if (res == 2) { // if entirely within frustrum discontinue culling
displayOctTreeCAll(node, center, size, minsize);
return;
}
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
for (short 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 || minsize > size ) { // if ith node is leaf get center
// check if leaf is visible
if ( CubeInFrustum(ccenter[0], ccenter[1], ccenter[2], size/2.0) ) {
showCube(ccenter, size/2.0);
}
} else { // recurse
displayOctTreeCAllCulled( children->node, ccenter, size/2.0, minsize);
}
++children; // next child
}
}
}
void compactTree::displayOctTreeCAll( cbitoct &node, double *center, double size, double minsize ) {
double ccenter[3];
cbitunion<tshort> *children;
cbitoct::getChildren(node, children);
for (short 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 || minsize > size ) { // if ith node is leaf get center
showCube(ccenter, size/2.0);
} else { // recurse
displayOctTreeCAll( children->node, ccenter, size/2.0, minsize);
}
++children; // next child
}
}
}
void compactTree::showCube(double *center, double size) {
glLineWidth(1.0);
glBegin(GL_QUADS); // draw a cube with 6 quads
glColor3f(0.0f,1.0f,0.0f); // Set The Color To Green
glVertex3f(center[0] + size, center[1] + size, center[2] - size);
glVertex3f(center[0] - size, center[1] + size, center[2] - size);
glVertex3f(center[0] - size, center[1] + size, center[2] + size);
glVertex3f(center[0] + size, center[1] + size, center[2] + size);
glColor3f(1.0f,0.5f,0.0f); // Set The Color To Orange
glVertex3f(center[0] + size, center[1] - size, center[2] + size);
glVertex3f(center[0] - size, center[1] - size, center[2] + size);
glVertex3f(center[0] - size, center[1] - size, center[2] - size);
glVertex3f(center[0] + size, center[1] - size, center[2] - size);
glColor3f(1.0f,0.0f,0.0f); // Set The Color To Red
glVertex3f(center[0] + size, center[1] + size, center[2] + size);
glVertex3f(center[0] - size, center[1] + size, center[2] + size);
glVertex3f(center[0] - size, center[1] - size, center[2] + size);
glVertex3f(center[0] + size, center[1] - size, center[2] + size);
glColor3f(1.0f,1.0f,0.0f); // Set The Color To Yellow
glVertex3f(center[0] + size, center[1] - size, center[2] - size);
glVertex3f(center[0] - size, center[1] - size, center[2] - size);
glVertex3f(center[0] - size, center[1] + size, center[2] - size);
glVertex3f(center[0] + size, center[1] + size, center[2] - size);
glColor3f(0.0f,0.0f,1.0f); // Set The Color To Blue
glVertex3f(center[0] - size, center[1] + size, center[2] + size);
glVertex3f(center[0] - size, center[1] + size, center[2] - size);
glVertex3f(center[0] - size, center[1] - size, center[2] - size);
glVertex3f(center[0] - size, center[1] - size, center[2] + size);
glColor3f(1.0f,0.0f,1.0f); // Set The Color To Violet
glVertex3f(center[0] + size, center[1] + size, center[2] - size);
glVertex3f(center[0] + size, center[1] + size, center[2] + size);
glVertex3f(center[0] + size, center[1] - size, center[2] + size);
glVertex3f(center[0] + size, center[1] - size, center[2] - size);
glEnd();
}
template <class T>
void compactTree::selectRay(vector<T *> &points) {
//selectRay(points, *root, center, size);
}
void compactTree::childcenter(double *pcenter, double *ccenter, double size, unsigned char i) {
switch (i) {
case 0:
ccenter[0] = pcenter[0] - size / 2.0;
ccenter[1] = pcenter[1] - size / 2.0;
ccenter[2] = pcenter[2] - size / 2.0;
break;
case 1:
ccenter[0] = pcenter[0] + size / 2.0;
ccenter[1] = pcenter[1] - size / 2.0;
ccenter[2] = pcenter[2] - size / 2.0;
break;
case 2:
ccenter[0] = pcenter[0] - size / 2.0;
ccenter[1] = pcenter[1] + size / 2.0;
ccenter[2] = pcenter[2] - size / 2.0;
break;
case 3:
ccenter[0] = pcenter[0] + size / 2.0;
ccenter[1] = pcenter[1] + size / 2.0;
ccenter[2] = pcenter[2] - size / 2.0;
break;
case 4:
ccenter[0] = pcenter[0] - size / 2.0;
ccenter[1] = pcenter[1] - size / 2.0;
ccenter[2] = pcenter[2] + size / 2.0;
break;
case 5:
ccenter[0] = pcenter[0] + size / 2.0;
ccenter[1] = pcenter[1] - size / 2.0;
ccenter[2] = pcenter[2] + size / 2.0;
break;
case 6:
ccenter[0] = pcenter[0] - size / 2.0;
ccenter[1] = pcenter[1] + size / 2.0;
ccenter[2] = pcenter[2] + size / 2.0;
break;
case 7:
ccenter[0] = pcenter[0] + size / 2.0;
ccenter[1] = pcenter[1] + size / 2.0;
ccenter[2] = pcenter[2] + size / 2.0;
break;
default:
break;
}
}
void compactTree::GetOctTreeCenter(vector<double*>&c) { GetOctTreeCenter(c, *root, center, size); }
void compactTree::AllPoints(vector<double *> &vp) { AllPoints(*compactTree::root, vp, center, size); }
long compactTree::countNodes() { return 1 + countNodes(*root); }
long compactTree::countLeaves() { return 1 + countLeaves(*root); }
void compactTree::setColorManager(ColorManager *_cm) { cm = _cm; }
void compactTree::drawLOD(float ratio) {
switch (current_lod_mode) {
case 1:
glBegin(GL_POINTS);
displayOctTreeCulledLOD2(ratio , *root, center, size);
glEnd();
break;
case 2:
/*
#ifdef WITH_GLEE
if (GLEE_ARB_point_parameters) {
glPointParameterfARB(GL_POINT_SIZE_MIN_ARB, 1.0);
glPointParameterfARB(GL_POINT_SIZE_MAX_ARB, 100000.0);
GLfloat p[3] = {0.0, 0.0000, 0.0000005};
glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p);
displayOctTreeCPAllCulled(*BOctTree<T>::root, BOctTree<T>::center, BOctTree<T>::size, BOctTree<T>::size/ pow(2, min( (int)(ratio * BOctTree<T>::max_depth ), BOctTree<T>::max_depth - 3) ) );
p[0] = 1.0;
p[2] = 0.0;
glPointParameterfvARB(GL_POINT_DISTANCE_ATTENUATION_ARB, p);
}
#endif
*/
//break;
case 0:
glBegin(GL_POINTS);
displayOctTreeCulledLOD(maxtargetpoints * ratio, *root, center, size);
glEnd();
break;
default:
break;
}
}
void compactTree::draw() {
displayOctTreeAllCulled(*root, center, size);
}
void compactTree::displayOctTree(double minsize ) {
displayOctTreeCAllCulled(*root, center, size, minsize);
}
shortpointrep* compactTree::createPoints(lint length) {
//shortpointrep *points = new shortpointrep[POINTDIM*length];
shortpointrep *points = alloc->allocate<shortpointrep> (POINTDIM*length);
return points;
}
void compactTree::deserialize(std::string filename ) {
char buffer[sizeof(float) * 20];
float *p = reinterpret_cast<float*>(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(float));
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;
float *fmins = new float[POINTDIM];
float *fmaxs = new float[POINTDIM];
mins = new double[POINTDIM];
maxs = new double[POINTDIM];
file.read(reinterpret_cast<char*>(fmins), POINTDIM * sizeof(float));
file.read(reinterpret_cast<char*>(fmaxs), POINTDIM * sizeof(float));
for (unsigned int i = 0; i < POINTDIM; i++) {
mins[i] = fmins[i];
maxs[i] = fmaxs[i];
}
double vs = size;
while (vs > voxelSize) {
vs = vs * 0.5;
}
// precision = vs / 32768; // 2^15
precision = vs / TSHORT_MAXP1; // 2^15
// read root node
//root = new cbitoct();
root = alloc->allocate<cbitoct>();
deserialize(file, *root );
file.close();
}
void compactTree::deserialize(std::ifstream &f, cbitoct &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
//cbitunion<tshort> *children = new cbitunion<tshort>[n_children];
cbitunion<tshort> *children = alloc->allocate<cbitunion<tshort> >(n_children);
cbitoct::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
lint length;
f.read(reinterpret_cast<char*>(&length), sizeof(lint));
shortpointrep *points = createPoints(length);
f.read(reinterpret_cast<char*>(points), sizeof(shortpointrep) * length * POINTDIM); // read the points
children->linkPoints(points, length);
} else { // write child
deserialize(f, children->node);
}
++children; // next child
}
}
}
void compactTree::serialize(std::string filename) {
char buffer[sizeof(float) * 20];
float *p = reinterpret_cast<float*>(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(float)]));
*ip = POINTDIM;
file.write(buffer, 5 * sizeof(float) + 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(float));
// write root node
serialize(file, *root);
file.close();
}
void compactTree::serialize(std::ofstream &of, cbitoct &node) {
char buffer[2];
buffer[0] = node.valid;
buffer[1] = node.leaf;
of.write(buffer, 2);
// write children
cbitunion<tshort> *children;
cbitoct::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
tshort *points = children->getPoints();
lint length = children->getLength();
of.write(reinterpret_cast<char*>(&length), sizeof(lint) );
of.write(reinterpret_cast<char*>(points), POINTDIM*length*sizeof(tshort) );
} else { // write child
serialize(of, children->node);
}
++children; // next child
}
}
}