849 lines
28 KiB
Text
849 lines
28 KiB
Text
|
/*
|
||
|
|
* 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
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|