diff --git a/io/src/openni_camera/openni_image_bayer_grbg.cpp b/io/src/openni_camera/openni_image_bayer_grbg.cpp index 7b6f3d05395..c1a625c6a25 100644 --- a/io/src/openni_camera/openni_image_bayer_grbg.cpp +++ b/io/src/openni_camera/openni_image_bayer_grbg.cpp @@ -41,10 +41,10 @@ #include #include -#define AVG(a,b) (((int)(a) + (int)(b)) >> 1) -#define AVG3(a,b,c) (((int)(a) + (int)(b) + (int)(c)) / 3) -#define AVG4(a,b,c,d) (((int)(a) + (int)(b) + (int)(c) + (int)(d)) >> 2) -#define WAVG4(a,b,c,d,x,y) ( ( ((int)(a) + (int)(b)) * (int)(x) + ((int)(c) + (int)(d)) * (int)(y) ) / ( ((int)(x) + (int(y))) << 1 ) ) +#define AVG(a,b) static_cast((int(a) + int(b)) >> 1) +#define AVG3(a,b,c) static_cast((int(a) + int(b) + (int)(c)) / 3) +#define AVG4(a,b,c,d) static_cast((int(a) + int(b) + int(c) + int(d)) >> 2) +#define WAVG4(a,b,c,d,x,y) static_cast( ( (int(a) + int(b)) * int(x) + (int(c) + int(d)) * int(y) ) / ( (int(x) + (int(y))) << 1 ) ) using namespace std; namespace openni_wrapper diff --git a/outofcore/src/cJSON.cpp b/outofcore/src/cJSON.cpp index a94b8c5d6ed..76399d6ecca 100644 --- a/outofcore/src/cJSON.cpp +++ b/outofcore/src/cJSON.cpp @@ -172,11 +172,11 @@ static const char *parse_string(cJSON *item,const char *str) len=3;if (uc<0x80) len=1;else if (uc<0x800) len=2;ptr2+=len; switch (len) { - case 3: *--ptr2 =(( (uc) | 0x80) & 0xBF ); + case 3: *--ptr2 = static_cast(( (uc) | 0x80) & 0xBF ); uc >>= 6; - case 2: *--ptr2 =(( (uc) | 0x80) & 0xBF ); + case 2: *--ptr2 = static_cast(( (uc) | 0x80) & 0xBF ); uc >>= 6; - case 1: *--ptr2 =( (uc) | firstByteMark[len] ); + case 1: *--ptr2 = static_cast( (uc) | firstByteMark[len] ); } ptr2+=len;ptr+=4; break; diff --git a/recognition/include/pcl/recognition/dense_quantized_multi_mod_template.h b/recognition/include/pcl/recognition/dense_quantized_multi_mod_template.h index 9cbdfb0847a..a65153d0b58 100644 --- a/recognition/include/pcl/recognition/dense_quantized_multi_mod_template.h +++ b/recognition/include/pcl/recognition/dense_quantized_multi_mod_template.h @@ -54,7 +54,7 @@ namespace pcl { const size_t num_of_features = static_cast (features.size ()); write (stream, num_of_features); - for (int feature_index = 0; feature_index < num_of_features; ++feature_index) + for (size_t feature_index = 0; feature_index < num_of_features; ++feature_index) { write (stream, features[feature_index]); } diff --git a/surface/include/pcl/surface/impl/poisson/multi_grid_octree_data.hpp b/surface/include/pcl/surface/impl/poisson/multi_grid_octree_data.hpp index 70bfe18bf27..b842f93630f 100644 --- a/surface/include/pcl/surface/impl/poisson/multi_grid_octree_data.hpp +++ b/surface/include/pcl/surface/impl/poisson/multi_grid_octree_data.hpp @@ -48,14 +48,17 @@ #define PAD_SIZE (Real (1.0)) -namespace pcl { - namespace poisson { +namespace pcl +{ + namespace poisson + { const Real EPSILON = Real (1e-6); const Real ROUND_EPS = Real (1e-5); ////////////////////////////////////////////////////////////////////////////////////////////// + SortedTreeNodes::SortedTreeNodes () { nodeCount = NULL; @@ -65,6 +68,7 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + SortedTreeNodes::~SortedTreeNodes () { if (nodeCount) @@ -77,67 +81,84 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// - void SortedTreeNodes::set (TreeOctNode& root,const int& setIndex) + + void + SortedTreeNodes::set (TreeOctNode& root, const int& setIndex) { if (nodeCount) delete[] nodeCount; if (treeNodes) delete[] treeNodes; - maxDepth = root.maxDepth ()+1; - nodeCount = new int[maxDepth+1]; + maxDepth = root.maxDepth () + 1; + nodeCount = new int[maxDepth + 1]; treeNodes = new TreeOctNode*[root.nodes ()]; TreeOctNode* temp = root.nextNode (); - int i,cnt = 0; - while (temp){ + int i, cnt = 0; + while (temp) + { treeNodes[cnt++] = temp; temp = root.nextNode (temp); } - qsort (treeNodes,cnt,sizeof (const TreeOctNode*),TreeOctNode::CompareForwardPointerDepths); - for (i = 0; i<= maxDepth; i++) + qsort (treeNodes, cnt, sizeof (const TreeOctNode*), TreeOctNode::CompareForwardPointerDepths); + for (i = 0; i <= maxDepth; i++) nodeCount[i] = 0; for (i = 0; i < cnt; i++) { if (setIndex) treeNodes[i]->nodeData.nodeIndex = i; - nodeCount[treeNodes[i]->depth ()+1]++; + nodeCount[treeNodes[i]->depth () + 1]++; } for (i = 1; i <= maxDepth; i++) - nodeCount[i] += nodeCount[i-1]; + nodeCount[i] += nodeCount[i - 1]; } ////////////////////////////////////////////////////////////////////////////////////////////// int TreeNodeData::UseIndex = 1; - TreeNodeData::TreeNodeData (){ - if (UseIndex){ + + TreeNodeData::TreeNodeData () + { + if (UseIndex) + { nodeIndex = -1; centerWeightContribution = 0; } - else{mcIndex = 0;} + else + { + mcIndex = 0; + } value = 0; } - TreeNodeData::~TreeNodeData (){;} + + TreeNodeData::~TreeNodeData () + { + ; + } ////////////////////////////////////////////////////////////////////////////////////////////// template - double Octree::maxMemoryUsage = 0; + double Octree::maxMemoryUsage = 0; ////////////////////////////////////////////////////////////////////////////////////////////// + template - double Octree::MemoryUsage () + double Octree + ::MemoryUsage () { - double mem = 0.0;//MemoryInfo::Usage ()/ (1<<20); + double mem = 0.0; //MemoryInfo::Usage ()/ (1<<20); if (mem > maxMemoryUsage) maxMemoryUsage = mem; return mem; } ////////////////////////////////////////////////////////////////////////////////////////////// + template - Octree::Octree () + Octree + ::Octree () { radius = 0; width = 0; @@ -146,20 +167,24 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::setNodeIndices (TreeOctNode& node, int& idx) + void Octree + ::setNodeIndices (TreeOctNode& node, int& idx) { node.nodeData.nodeIndex = idx; idx++; if (node.children) for (int i = 0; i < Cube::CORNERS; i++) - setNodeIndices (node.children[i],idx); + setNodeIndices (node.children[i], idx); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::NonLinearSplatOrientedPoint (TreeOctNode* node,const Point3D& position,const Point3D& normal) + int Octree + ::NonLinearSplatOrientedPoint (TreeOctNode* node, const Point3D& position, const Point3D& normal) { double x, dxdy, dxdydz, dx[DIMENSION][3]; int i, j, k; @@ -172,9 +197,9 @@ namespace pcl { width = w; for (int i = 0; i < 3; i++) { - x = (center.coords[i]-position.coords[i]-width)/width; - dx[i][0] = 1.125+1.500*x+0.500*x*x; - x = (center.coords[i]-position.coords[i])/width; + x = (center.coords[i] - position.coords[i] - width) / width; + dx[i][0] = 1.125 + 1.500 * x + 0.500 * x*x; + x = (center.coords[i] - position.coords[i]) / width; dx[i][1] = 0.750 - x*x; dx[i][2] = 1.0 - dx[i][1] - dx[i][0]; } @@ -183,12 +208,12 @@ namespace pcl { { for (j = 0; j < 3; j++) { - dxdy = dx[0][i]*dx[1][j]; + dxdy = dx[0][i] * dx[1][j]; for (k = 0; k < 3; k++) { if (neighbors.neighbors[i][j][k]) { - dxdydz = dxdy*dx[2][k]; + dxdydz = dxdy * dx[2][k]; int idx = neighbors.neighbors[i][j][k]->nodeData.nodeIndex; if (idx < 0) { @@ -197,9 +222,9 @@ namespace pcl { idx = neighbors.neighbors[i][j][k]->nodeData.nodeIndex = int (normals->size ()); normals->push_back (n); } - (*normals)[idx].coords[0] += Real (normal.coords[0]*dxdydz); - (*normals)[idx].coords[1] += Real (normal.coords[1]*dxdydz); - (*normals)[idx].coords[2] += Real (normal.coords[2]*dxdydz); + (*normals)[idx].coords[0] += Real (normal.coords[0] * dxdydz); + (*normals)[idx].coords[1] += Real (normal.coords[1] * dxdydz); + (*normals)[idx].coords[2] += Real (normal.coords[2] * dxdydz); } } } @@ -209,18 +234,20 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::NonLinearSplatOrientedPoint (const Point3D& position, - const Point3D& normal, - const int& splatDepth, - const Real& samplesPerNode, - const int& minDepth, - const int& maxDepth) + void Octree + ::NonLinearSplatOrientedPoint (const Point3D& position, + const Point3D& normal, + const int& splatDepth, + const Real& samplesPerNode, + const int& minDepth, + const int& maxDepth) { double dx; Point3D n; TreeOctNode* temp; - int i,cnt = 0; + int i; //,cnt = 0; double width; Point3D myCenter; Real myWidth; @@ -228,7 +255,7 @@ namespace pcl { myWidth = Real (1.0); temp = &tree; - while (temp->depth ()depth () < splatDepth) { if (!temp->children) { @@ -239,23 +266,23 @@ namespace pcl { int cIndex = TreeOctNode::CornerIndex (myCenter, position); temp = &temp->children[cIndex]; myWidth /= 2; - if (cIndex&1) - myCenter.coords[0] += myWidth/2; + if (cIndex & 1) + myCenter.coords[0] += myWidth / 2; else - myCenter.coords[0] -= myWidth/2; + myCenter.coords[0] -= myWidth / 2; - if (cIndex&2) - myCenter.coords[1] += myWidth/2; + if (cIndex & 2) + myCenter.coords[1] += myWidth / 2; else - myCenter.coords[1] -= myWidth/2; + myCenter.coords[1] -= myWidth / 2; - if (cIndex&4) - myCenter.coords[2] += myWidth/2; + if (cIndex & 4) + myCenter.coords[2] += myWidth / 2; else - myCenter.coords[2] -= myWidth/2; + myCenter.coords[2] -= myWidth / 2; } - Real alpha,newDepth; + Real alpha, newDepth; NonLinearGetSampleDepthAndWeight (temp, position, samplesPerNode, newDepth, alpha); if (newDepth < minDepth) @@ -265,13 +292,13 @@ namespace pcl { int topDepth = int (ceil (newDepth)); - dx = 1.0- (topDepth-newDepth); - if (topDepth<= minDepth) + dx = 1.0 - (topDepth - newDepth); + if (topDepth <= minDepth) { topDepth = minDepth; dx = 1; } - else if (topDepth>maxDepth) + else if (topDepth > maxDepth) { topDepth = maxDepth; dx = 1; @@ -285,68 +312,72 @@ namespace pcl { int cIndex = TreeOctNode::CornerIndex (myCenter, position); temp = &temp->children[cIndex]; myWidth /= 2; - if (cIndex&1) - myCenter.coords[0]+= myWidth/2; + if (cIndex & 1) + myCenter.coords[0] += myWidth / 2; else - myCenter.coords[0]-= myWidth/2; - if (cIndex&2) - myCenter.coords[1]+= myWidth/2; + myCenter.coords[0] -= myWidth / 2; + if (cIndex & 2) + myCenter.coords[1] += myWidth / 2; else - myCenter.coords[1]-= myWidth/2; - if (cIndex&4) - myCenter.coords[2]+= myWidth/2; + myCenter.coords[1] -= myWidth / 2; + if (cIndex & 4) + myCenter.coords[2] += myWidth / 2; else - myCenter.coords[2]-= myWidth/2; + myCenter.coords[2] -= myWidth / 2; } - width = 1.0 / (1<depth ()); + width = 1.0 / (1 << temp->depth ()); for (i = 0; i < DIMENSION; i++) - n.coords[i] = normal.coords[i]*alpha / Real (pow (width,3))*Real (dx); + n.coords[i] = normal.coords[i] * alpha / Real (pow (width, 3)) * Real (dx); NonLinearSplatOrientedPoint (temp, position, n); - if (fabs (1.0-dx) > EPSILON) + if (fabs (1.0 - dx) > EPSILON) { - dx = Real (1.0-dx); + dx = Real (1.0 - dx); temp = temp->parent; - width = 1.0/ (1<depth ()); + width = 1.0 / (1 << temp->depth ()); for (i = 0; i < DIMENSION; i++) - n.coords[i] = normal.coords[i]*alpha/Real (pow (width,3))*Real (dx); + n.coords[i] = normal.coords[i] * alpha / Real (pow (width, 3)) * Real (dx); NonLinearSplatOrientedPoint (temp, position, n); } } ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::NonLinearGetSampleDepthAndWeight (TreeOctNode* node, - const Point3D& position, - const Real& samplesPerNode, - Real& depth, - Real& weight) + void Octree + ::NonLinearGetSampleDepthAndWeight (TreeOctNode* node, + const Point3D& position, + const Real& samplesPerNode, + Real& depth, + Real& weight) { TreeOctNode* temp = node; - weight = Real (1.0)/NonLinearGetSampleWeight (temp, position); - if (weight >= samplesPerNode+1) - depth = Real (temp->depth () + log (weight/ (samplesPerNode+1))/log (double (1<< (DIMENSION-1)))); + weight = Real (1.0) / NonLinearGetSampleWeight (temp, position); + if (weight >= samplesPerNode + 1) + depth = Real (temp->depth () + log (weight / (samplesPerNode + 1)) / log (double (1 << (DIMENSION - 1)))); else { - Real oldAlpha,newAlpha; + Real oldAlpha, newAlpha; oldAlpha = newAlpha = weight; - while (newAlpha < (samplesPerNode+1) && temp->parent) + while (newAlpha < (samplesPerNode + 1) && temp->parent) { temp = temp->parent; oldAlpha = newAlpha; - newAlpha = Real (1.0)/NonLinearGetSampleWeight (temp,position); + newAlpha = Real (1.0) / NonLinearGetSampleWeight (temp, position); } - depth = Real (temp->depth ()+log (newAlpha/ (samplesPerNode+1))/log (newAlpha/oldAlpha)); + depth = Real (temp->depth () + log (newAlpha / (samplesPerNode + 1)) / log (newAlpha / oldAlpha)); } - weight = Real (pow (double (1<< (DIMENSION-1)),-double (depth))); + weight = Real (pow (double (1 << (DIMENSION - 1)), -double (depth))); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - Real Octree::NonLinearGetSampleWeight (TreeOctNode* node, - const Point3D& position) + Real Octree + ::NonLinearGetSampleWeight (TreeOctNode* node, + const Point3D& position) { Real weight = 0; double x, dxdy, dx[DIMENSION][3]; @@ -360,34 +391,36 @@ namespace pcl { for (i = 0; i < DIMENSION; i++) { - x = (center.coords[i]-position.coords[i]-width) / width; - dx[i][0] = 1.125+1.500*x+0.500*x*x; - x = (center.coords[i]-position.coords[i]) / width; + x = (center.coords[i] - position.coords[i] - width) / width; + dx[i][0] = 1.125 + 1.500 * x + 0.500 * x*x; + x = (center.coords[i] - position.coords[i]) / width; dx[i][1] = 0.750 - x*x; - dx[i][2] = 1.0-dx[i][1]-dx[i][0]; + dx[i][2] = 1.0 - dx[i][1] - dx[i][0]; } for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { - dxdy = dx[0][i]*dx[1][j]; + dxdy = dx[0][i] * dx[1][j]; for (k = 0; k < 3; k++) { if (neighbors.neighbors[i][j][k]) - weight += Real (dxdy*dx[2][k]*neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution); + weight += Real (dxdy * dx[2][k] * neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution); } } } - return Real (1.0/weight); + return Real (1.0 / weight); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::NonLinearUpdateWeightContribution (TreeOctNode* node, - const Point3D& position, - const Real& weight) + int Octree + ::NonLinearUpdateWeightContribution (TreeOctNode* node, + const Point3D& position, + const Real& weight) { int i, j, k; TreeOctNode::Neighbors& neighbors = neighborKey.setNeighbors (node); @@ -400,21 +433,21 @@ namespace pcl { for (i = 0; i < DIMENSION; i++) { - x = (center.coords[i]-position.coords[i]-width)/width; - dx[i][0] = 1.125+1.500*x+0.500*x*x; - x = (center.coords[i]-position.coords[i])/width; + x = (center.coords[i] - position.coords[i] - width) / width; + dx[i][0] = 1.125 + 1.500 * x + 0.500 * x*x; + x = (center.coords[i] - position.coords[i]) / width; dx[i][1] = 0.750 - x*x; - dx[i][2] = 1.0-dx[i][1]-dx[i][0]; + dx[i][2] = 1.0 - dx[i][1] - dx[i][0]; } for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { - dxdy = dx[0][i]*dx[1][j]*weight; + dxdy = dx[0][i] * dx[1][j] * weight; for (k = 0; k < 3; k++) if (neighbors.neighbors[i][j][k]) - neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution += Real (dxdy*dx[2][k]); + neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution += Real (dxdy * dx[2][k]); } } return 0; @@ -422,23 +455,25 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + template template int - Octree::setTree (boost::shared_ptr > input_, - const int& maxDepth, - const int& kernelDepth, - const Real& samplesPerNode, - const Real& scaleFactor, - Point3D& center, - Real& scale, - const int& resetSamples, - const int& useConfidence) - { - Point3D min,max,position,normal,myCenter; + Octree + ::setTree (boost::shared_ptr > input_, + const int& maxDepth, + const int& kernelDepth, + const Real& samplesPerNode, + const Real& scaleFactor, + Point3D& center, + Real& scale, + const int& resetSamples, + const int& useConfidence) + { + Point3D min, max, position, normal, myCenter; Real myWidth; - int i,cnt = 0; + size_t i, cnt = 0; TreeOctNode* temp; int splatDepth = 0; - float c[2*DIMENSION]; + float c[2 * DIMENSION]; TreeNodeData::UseIndex = 1; neighborKey.set (maxDepth); @@ -449,7 +484,7 @@ namespace pcl { // Read through once to get the center and scale while (1) { - if (cnt == input_->size ()) + if (cnt == input_->size ()) break; c[0] = input_->points[cnt].x; c[1] = input_->points[cnt].y; @@ -460,24 +495,24 @@ namespace pcl { for (i = 0; i < DIMENSION; i++) { - if (!cnt || c[i]max.coords[i]) + if (!cnt || c[i] > max.coords[i]) max.coords[i] = c[i]; } - cnt ++; + cnt++; } for (i = 0; i < DIMENSION; i++) { - if (!i || scale < max.coords[i]-min.coords[i]) - scale = Real (max.coords[i]-min.coords[i]); - center.coords[i] = Real (max.coords[i]+min.coords[i])/2; + if (!i || scale < max.coords[i] - min.coords[i]) + scale = Real (max.coords[i] - min.coords[i]); + center.coords[i] = Real (max.coords[i] + min.coords[i]) / 2; } scale *= scaleFactor; for (i = 0; i < DIMENSION; i++) - center.coords[i]-= scale/2; + center.coords[i] -= scale / 2; if (splatDepth > 0) { cnt = 0; @@ -494,13 +529,13 @@ namespace pcl { for (i = 0; i < DIMENSION; i++) { - position.coords[i] = (c[i]-center.coords[i]) / scale; - normal.coords[i] = c[DIMENSION+i]; + position.coords[i] = (c[i] - center.coords[i]) / scale; + normal.coords[i] = c[DIMENSION + i]; } myCenter.coords[0] = myCenter.coords[1] = myCenter.coords[2] = Real (0.5); myWidth = Real (1.0); for (i = 0; i < DIMENSION; i++) - if (position.coords[i] < myCenter.coords[i]-myWidth/2 || position.coords[i]>myCenter.coords[i]+myWidth/2) + if (position.coords[i] < myCenter.coords[i] - myWidth / 2 || position.coords[i] > myCenter.coords[i] + myWidth / 2) break; if (i != DIMENSION) continue; @@ -513,28 +548,28 @@ namespace pcl { while (d < splatDepth) { - NonLinearUpdateWeightContribution (temp,position,weight); + NonLinearUpdateWeightContribution (temp, position, weight); if (!temp->children) temp->initChildren (); - int cIndex = TreeOctNode::CornerIndex (myCenter,position); + int cIndex = TreeOctNode::CornerIndex (myCenter, position); temp = &temp->children[cIndex]; - myWidth/= 2; - if (cIndex&1) - myCenter.coords[0] += myWidth/2; + myWidth /= 2; + if (cIndex & 1) + myCenter.coords[0] += myWidth / 2; else - myCenter.coords[0] -= myWidth/2; - if (cIndex&2) - myCenter.coords[1] += myWidth/2; + myCenter.coords[0] -= myWidth / 2; + if (cIndex & 2) + myCenter.coords[1] += myWidth / 2; else - myCenter.coords[1] -= myWidth/2; - if (cIndex&4) - myCenter.coords[2] += myWidth/2; + myCenter.coords[1] -= myWidth / 2; + if (cIndex & 4) + myCenter.coords[2] += myWidth / 2; else - myCenter.coords[2] -= myWidth/2; - d ++; + myCenter.coords[2] -= myWidth / 2; + d++; } NonLinearUpdateWeightContribution (temp, position, weight); - cnt ++; + cnt++; } } @@ -550,18 +585,18 @@ namespace pcl { c[3] = input_->points[cnt].normal_x; c[4] = input_->points[cnt].normal_y; c[5] = input_->points[cnt].normal_z; - cnt ++; + cnt++; for (i = 0; i < DIMENSION; i++) { - position.coords[i] = (c[i]-center.coords[i])/scale; - normal.coords[i] = c[DIMENSION+i]; + position.coords[i] = (c[i] - center.coords[i]) / scale; + normal.coords[i] = c[DIMENSION + i]; } myCenter.coords[0] = myCenter.coords[1] = myCenter.coords[2] = Real (0.5); myWidth = Real (1.0); for (i = 0; i < DIMENSION; i++) - if (position.coords[i] < myCenter.coords[i]-myWidth/2 || position.coords[i] > myCenter.coords[i]+myWidth/2) + if (position.coords[i] < myCenter.coords[i] - myWidth / 2 || position.coords[i] > myCenter.coords[i] + myWidth / 2) break; if (i != DIMENSION) continue; @@ -574,13 +609,13 @@ namespace pcl { normal.coords[1] /= l; normal.coords[2] /= l; } - l = Real (2<0 && splatDepth) - NonLinearSplatOrientedPoint (position,normal,splatDepth,samplesPerNode,1,maxDepth); + if (resetSamples && samplesPerNode > 0 && splatDepth) + NonLinearSplatOrientedPoint (position, normal, splatDepth, samplesPerNode, 1, maxDepth); else { Real alpha = 1; @@ -590,22 +625,22 @@ namespace pcl { { while (d < splatDepth) { - int cIndex = TreeOctNode::CornerIndex (myCenter,position); + int cIndex = TreeOctNode::CornerIndex (myCenter, position); temp = &temp->children[cIndex]; - myWidth/= 2; - if (cIndex&1) - myCenter.coords[0] += myWidth/2; + myWidth /= 2; + if (cIndex & 1) + myCenter.coords[0] += myWidth / 2; else - myCenter.coords[0] -= myWidth/2; - if (cIndex&2) - myCenter.coords[1] += myWidth/2; + myCenter.coords[0] -= myWidth / 2; + if (cIndex & 2) + myCenter.coords[1] += myWidth / 2; else - myCenter.coords[1] -= myWidth/2; - if (cIndex&4) - myCenter.coords[2] += myWidth/2; + myCenter.coords[1] -= myWidth / 2; + if (cIndex & 4) + myCenter.coords[2] += myWidth / 2; else - myCenter.coords[2]-= myWidth/2; - d ++; + myCenter.coords[2] -= myWidth / 2; + d++; } alpha = NonLinearGetSampleWeight (temp, position); } @@ -618,18 +653,18 @@ namespace pcl { int cIndex = TreeOctNode::CornerIndex (myCenter, position); temp = &temp->children[cIndex]; myWidth /= 2; - if (cIndex&1) - myCenter.coords[0] += myWidth/2; + if (cIndex & 1) + myCenter.coords[0] += myWidth / 2; else - myCenter.coords[0] -= myWidth/2; - if (cIndex&2) - myCenter.coords[1] += myWidth/2; + myCenter.coords[0] -= myWidth / 2; + if (cIndex & 2) + myCenter.coords[1] += myWidth / 2; else - myCenter.coords[1] -= myWidth/2; - if (cIndex&4) - myCenter.coords[2] += myWidth/2; + myCenter.coords[1] -= myWidth / 2; + if (cIndex & 4) + myCenter.coords[2] += myWidth / 2; else - myCenter.coords[2] -= myWidth/2; + myCenter.coords[2] -= myWidth / 2; d++; } NonLinearSplatOrientedPoint (temp, position, normal); @@ -640,22 +675,26 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::setFunctionData (const PPolynomial& ReconstructionFunction, - const int& maxDepth,const int& normalize, - const Real& normalSmooth) + void Octree + ::setFunctionData (const PPolynomial& ReconstructionFunction, + const int& maxDepth, const int& normalize, + const Real& normalSmooth) { radius = Real (fabs (ReconstructionFunction.polys[0].start)); - width = int (double (radius+0.5-EPSILON)*2); + width = int (double (radius + 0.5 - EPSILON)*2); if (normalSmooth > 0) postNormalSmooth = normalSmooth; - fData.set (maxDepth,ReconstructionFunction,normalize,1); + fData.set (maxDepth, ReconstructionFunction, normalize, 1); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::finalize1 (const int& refineNeighbors) + void Octree + ::finalize1 (const int& refineNeighbors) { TreeOctNode* temp; @@ -665,10 +704,10 @@ namespace pcl { temp = tree.nextNode (); while (temp) { - if (temp->nodeData.nodeIndex>= 0 && Length ( (*normals)[temp->nodeData.nodeIndex])>EPSILON) + if (temp->nodeData.nodeIndex >= 0 && Length ((*normals)[temp->nodeData.nodeIndex]) > EPSILON) { - rf.depth = temp->depth ()-refineNeighbors; - TreeOctNode::ProcessMaxDepthNodeAdjacentNodes (fData.depth,temp,2*width,&tree,1,temp->depth ()-refineNeighbors,&rf); + rf.depth = temp->depth () - refineNeighbors; + TreeOctNode::ProcessMaxDepthNodeAdjacentNodes (fData.depth, temp, 2 * width, &tree, 1, temp->depth () - refineNeighbors, &rf); } temp = tree.nextNode (temp); } @@ -678,7 +717,7 @@ namespace pcl { temp = tree.nextLeaf (); while (temp) { - if (!temp->children && temp->depth ()children && temp->depth () < fData.depth) temp->initChildren (); temp = tree.nextLeaf (temp); } @@ -687,8 +726,10 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::finalize2 (const int& refineNeighbors) + void Octree + ::finalize2 (const int& refineNeighbors) { TreeOctNode* temp; @@ -700,8 +741,8 @@ namespace pcl { { if (fabs (temp->nodeData.value) > EPSILON) { - rf.depth = temp->depth ()-refineNeighbors; - TreeOctNode::ProcessMaxDepthNodeAdjacentNodes (fData.depth,temp,2*width,&tree,1,temp->depth ()-refineNeighbors,&rf); + rf.depth = temp->depth () - refineNeighbors; + TreeOctNode::ProcessMaxDepthNodeAdjacentNodes (fData.depth, temp, 2 * width, &tree, 1, temp->depth () - refineNeighbors, &rf); } temp = tree.nextNode (temp); } @@ -710,42 +751,50 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - Real Octree::GetDivergence (const int idx[DIMENSION], const Point3D& normal) const + Real Octree + ::GetDivergence (const int idx[DIMENSION], const Point3D& normal) const { - double dot = fData.dotTable[idx[0]]*fData.dotTable[idx[1]]*fData.dotTable[idx[2]]; - return Real (dot* (fData.dDotTable[idx[0]]*normal.coords[0]+fData.dDotTable[idx[1]]*normal.coords[1]+fData.dDotTable[idx[2]]*normal.coords[2])); + double dot = fData.dotTable[idx[0]] * fData.dotTable[idx[1]] * fData.dotTable[idx[2]]; + return Real (dot * (fData.dDotTable[idx[0]] * normal.coords[0] + fData.dDotTable[idx[1]] * normal.coords[1] + fData.dDotTable[idx[2]] * normal.coords[2])); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - Real Octree::GetLaplacian (const int idx[DIMENSION]) const + Real Octree + ::GetLaplacian (const int idx[DIMENSION]) const { - return Real (fData.dotTable[idx[0]]*fData.dotTable[idx[1]]*fData.dotTable[idx[2]]* (fData.d2DotTable[idx[0]]+fData.d2DotTable[idx[1]]+fData.d2DotTable[idx[2]])); + return Real (fData.dotTable[idx[0]] * fData.dotTable[idx[1]] * fData.dotTable[idx[2]]* (fData.d2DotTable[idx[0]] + fData.d2DotTable[idx[1]] + fData.d2DotTable[idx[2]])); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - Real Octree::GetDotProduct (const int idx[DIMENSION]) const + Real Octree + ::GetDotProduct (const int idx[DIMENSION]) const { - return Real (fData.dotTable[idx[0]]*fData.dotTable[idx[1]]*fData.dotTable[idx[2]]); + return Real (fData.dotTable[idx[0]] * fData.dotTable[idx[1]] * fData.dotTable[idx[2]]); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::GetFixedDepthLaplacian (SparseSymmetricMatrix& matrix, - const int& depth, - const SortedTreeNodes& sNodes) + int Octree + ::GetFixedDepthLaplacian (SparseSymmetricMatrix& matrix, + const int& depth, + const SortedTreeNodes& sNodes) { LaplacianMatrixFunction mf; mf.ot = this; mf.offset = sNodes.nodeCount[depth]; - matrix.Resize (sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth]); - mf.rowElements = (MatrixEntry*)malloc (sizeof (MatrixEntry)*matrix.rows); - for (int i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth+1]; i++) + matrix.Resize (sNodes.nodeCount[depth + 1] - sNodes.nodeCount[depth]); + mf.rowElements = (MatrixEntry*)malloc (sizeof (MatrixEntry) * matrix.rows); + for (int i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth + 1]; i++) { mf.elementCount = 0; mf.d2 = int (sNodes.treeNodes[i]->d); @@ -755,9 +804,9 @@ namespace pcl { mf.index[0] = mf.x2; mf.index[1] = mf.y2; mf.index[2] = mf.z2; - TreeOctNode::ProcessTerminatingNodeAdjacentNodes (fData.depth,sNodes.treeNodes[i],2*width-1,&tree,1,&mf); - matrix.SetRowSize (i-sNodes.nodeCount[depth],mf.elementCount); - memcpy (matrix.m_ppElements[i-sNodes.nodeCount[depth]],mf.rowElements,sizeof (MatrixEntry)*mf.elementCount); + TreeOctNode::ProcessTerminatingNodeAdjacentNodes (fData.depth, sNodes.treeNodes[i], 2 * width - 1, &tree, 1, &mf); + matrix.SetRowSize (i - sNodes.nodeCount[depth], mf.elementCount); + memcpy (matrix.m_ppElements[i - sNodes.nodeCount[depth]], mf.rowElements, sizeof (MatrixEntry) * mf.elementCount); } free (mf.rowElements); return 1; @@ -765,23 +814,25 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::GetRestrictedFixedDepthLaplacian (SparseSymmetricMatrix& matrix, - const int& depth, - const int* entries, - const int& entryCount, - const TreeOctNode* rNode, - const Real& radius, - const SortedTreeNodes& sNodes) + int Octree + ::GetRestrictedFixedDepthLaplacian (SparseSymmetricMatrix& matrix, + const int& /*depth*/, + const int* entries, + const int& entryCount, + const TreeOctNode* rNode, + const Real& radius, + const SortedTreeNodes& sNodes) { int i; RestrictedLaplacianMatrixFunction mf; - Real myRadius = int (2*radius-ROUND_EPS)+ROUND_EPS; + //Real myRadius = int (2*radius-ROUND_EPS)+ROUND_EPS; mf.ot = this; mf.radius = radius; - rNode->depthAndOffset (mf.depth,mf.offset); + rNode->depthAndOffset (mf.depth, mf.offset); matrix.Resize (entryCount); - mf.rowElements = (MatrixEntry*)malloc (sizeof (MatrixEntry)*matrix.rows); + mf.rowElements = (MatrixEntry*)malloc (sizeof (MatrixEntry) * matrix.rows); for (i = 0; i < entryCount; i++) sNodes.treeNodes[entries[i]]->nodeData.nodeIndex = i; @@ -791,9 +842,9 @@ namespace pcl { mf.index[0] = int (sNodes.treeNodes[entries[i]]->off[0]); mf.index[1] = int (sNodes.treeNodes[entries[i]]->off[1]); mf.index[2] = int (sNodes.treeNodes[entries[i]]->off[2]); - TreeOctNode::ProcessTerminatingNodeAdjacentNodes (fData.depth,sNodes.treeNodes[entries[i]],2*width-1,&tree,1,&mf); - matrix.SetRowSize (i,mf.elementCount); - memcpy (matrix.m_ppElements[i],mf.rowElements,sizeof (MatrixEntry)*mf.elementCount); + TreeOctNode::ProcessTerminatingNodeAdjacentNodes (fData.depth, sNodes.treeNodes[entries[i]], 2 * width - 1, &tree, 1, &mf); + matrix.SetRowSize (i, mf.elementCount); + memcpy (matrix.m_ppElements[i], mf.rowElements, sizeof (MatrixEntry) * mf.elementCount); } for (i = 0; i < entryCount; i++) sNodes.treeNodes[entries[i]]->nodeData.nodeIndex = entries[i]; @@ -803,14 +854,16 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::LaplacianMatrixIteration (const int& subdivideDepth) + int Octree + ::LaplacianMatrixIteration (const int& subdivideDepth) { int i, iter = 0; SortedTreeNodes sNodes; - double t; + //double t; fData.setDotTables (fData.D2_DOT_FLAG); - sNodes.set (tree,1); + sNodes.set (tree, 1); SparseMatrix::SetAllocator (MEMORY_ALLOCATOR_BLOCK_SIZE); @@ -818,9 +871,9 @@ namespace pcl { for (i = 1; i < sNodes.maxDepth; i++) { if (subdivideDepth > 0) - iter += SolveFixedDepthMatrix (i,subdivideDepth,sNodes); + iter += SolveFixedDepthMatrix (i, subdivideDepth, sNodes); else - iter += SolveFixedDepthMatrix (i,sNodes); + iter += SolveFixedDepthMatrix (i, sNodes); } SparseMatrix::AllocatorMatrixEntry.reset (); @@ -830,41 +883,43 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::SolveFixedDepthMatrix (const int& depth,const SortedTreeNodes& sNodes) + int Octree + ::SolveFixedDepthMatrix (const int& depth, const SortedTreeNodes& sNodes) { - int i,iter = 0; - Vector V,Solution; + int i, iter = 0; + Vector V, Solution; SparseSymmetricMatrix matrix; Real myRadius; Real dx, dy, dz; - int x1, x2 ,y1 ,y2 ,z1 ,z2; + int x1, x2, y1, y2, z1, z2; Vector Diagonal; - V.Resize (sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth]); - for (i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth+1]; i++) - V[i-sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.value; + V.Resize (sNodes.nodeCount[depth + 1] - sNodes.nodeCount[depth]); + for (i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth + 1]; i++) + V[i - sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.value; SparseSymmetricMatrix::AllocatorMatrixEntry.rollBack (); - GetFixedDepthLaplacian (matrix,depth,sNodes); - iter+= SparseSymmetricMatrix::Solve (matrix,V,int (pow (matrix.rows,ITERATION_POWER)),Solution,double (EPSILON),1); + GetFixedDepthLaplacian (matrix, depth, sNodes); + iter += SparseSymmetricMatrix::Solve (matrix, V, int (pow (matrix.rows, ITERATION_POWER)), Solution, double (EPSILON), 1); - for (i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth+1]; i++) - sNodes.treeNodes[i]->nodeData.value = Real (Solution[i-sNodes.nodeCount[depth]]); + for (i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth + 1]; i++) + sNodes.treeNodes[i]->nodeData.value = Real (Solution[i - sNodes.nodeCount[depth]]); - myRadius = Real (radius+ROUND_EPS-0.5); - myRadius /= (1<children) continue; x1 = int (node1->off[0]); @@ -873,7 +928,7 @@ namespace pcl { for (int j = 0; j < matrix.rowSizes[i]; j++) { idx2 = matrix.m_ppElements[i][j].N; - node2 = sNodes.treeNodes[idx2+off]; + node2 = sNodes.treeNodes[idx2 + off]; x2 = int (node2->off[0]); y2 = int (node2->off[1]); z2 = int (node2->off[2]); @@ -881,11 +936,11 @@ namespace pcl { pf.index[0] = x2; pf.index[1] = y2; pf.index[2] = z2; - dx = Real (x2-x1)/ (1<processNodeNodes (node2,&pf,0); + node1->processNodeNodes (node2, &pf, 0); else TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, node2, width, node1, width, &pf, 0); } @@ -894,7 +949,7 @@ namespace pcl { for (i = 0; i < matrix.rows; i++) { idx1 = i; - node1 = sNodes.treeNodes[idx1+off]; + node1 = sNodes.treeNodes[idx1 + off]; x1 = int (node1->off[0]); y1 = int (node1->off[1]); z1 = int (node1->off[2]); @@ -905,18 +960,19 @@ namespace pcl { for (int j = 0; j < matrix.rowSizes[i]; j++) { idx2 = matrix.m_ppElements[i][j].N; - node2 = sNodes.treeNodes[idx2+off]; - if (idx1!= idx2 && node2->children){ + node2 = sNodes.treeNodes[idx2 + off]; + if (idx1 != idx2 && node2->children) + { x2 = int (node2->off[0]); y2 = int (node2->off[1]); z2 = int (node2->off[2]); - dx = Real (x1-x2)/ (1<processNodeNodes (node1,&pf,0); + node2->processNodeNodes (node1, &pf, 0); else - TreeOctNode::ProcessNodeAdjacentNodes (fData.depth,node1,width,node2,width,&pf,0); + TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, node1, width, node2, width, &pf, 0); } } } @@ -926,10 +982,12 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::SolveFixedDepthMatrix (const int& depth, - const int& startingDepth, - const SortedTreeNodes& sNodes) + int Octree + ::SolveFixedDepthMatrix (const int& depth, + const int& startingDepth, + const SortedTreeNodes& sNodes) { int i, j, d, iter = 0; SparseSymmetricMatrix matrix; @@ -942,21 +1000,21 @@ namespace pcl { Vector Diagonal; if (startingDepth >= depth) - return SolveFixedDepthMatrix (depth,sNodes); + return SolveFixedDepthMatrix (depth, sNodes); - Values.Resize (sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth]); + Values.Resize (sNodes.nodeCount[depth + 1] - sNodes.nodeCount[depth]); - for (i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth+1]; i++) + for (i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth + 1]; i++) { - Values[i-sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.value; + Values[i - sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.value; sNodes.treeNodes[i]->nodeData.value = 0; } - myRadius = 2*radius-Real (0.5); - myRadius = int (myRadius-ROUND_EPS)+ROUND_EPS; - myRadius2 = Real (radius+ROUND_EPS-0.5); - d = depth-startingDepth; - for (i = sNodes.nodeCount[d]; i < sNodes.nodeCount[d+1]; i++) + myRadius = 2 * radius - Real (0.5); + myRadius = int (myRadius - ROUND_EPS) + ROUND_EPS; + myRadius2 = Real (radius + ROUND_EPS - 0.5); + d = depth - startingDepth; + for (i = sNodes.nodeCount[d]; i < sNodes.nodeCount[d + 1]; i++) { TreeOctNode* temp; // Get all of the entries associated to the subspace @@ -966,17 +1024,17 @@ namespace pcl { { if (temp->depth () == depth) { - acf.Function (temp,temp); + acf.Function (temp, temp); temp = sNodes.treeNodes[i]->nextBranch (temp); } else temp = sNodes.treeNodes[i]->nextNode (temp); } - for (j = sNodes.nodeCount[d]; j < sNodes.nodeCount[d+1]; j++) + for (j = sNodes.nodeCount[d]; j < sNodes.nodeCount[d + 1]; j++) { if (i == j) continue; - TreeOctNode::ProcessFixedDepthNodeAdjacentNodes (fData.depth,sNodes.treeNodes[i],1,sNodes.treeNodes[j],2*width-1,depth,&acf); + TreeOctNode::ProcessFixedDepthNodeAdjacentNodes (fData.depth, sNodes.treeNodes[i], 1, sNodes.treeNodes[j], 2 * width - 1, depth, &acf); } if (!acf.adjacencyCount) @@ -985,35 +1043,36 @@ namespace pcl { asf.adjacencyCount = 0; temp = sNodes.treeNodes[i]->nextNode (); while (temp - ){ + ) + { if (temp->depth () == depth) { - asf.Function (temp,temp); + asf.Function (temp, temp); temp = sNodes.treeNodes[i]->nextBranch (temp); } else temp = sNodes.treeNodes[i]->nextNode (temp); } - for (j = sNodes.nodeCount[d]; j < sNodes.nodeCount[d+1]; j++) + for (j = sNodes.nodeCount[d]; j < sNodes.nodeCount[d + 1]; j++) { if (i == j) continue; - TreeOctNode::ProcessFixedDepthNodeAdjacentNodes (fData.depth,sNodes.treeNodes[i],1,sNodes.treeNodes[j],2*width-1,depth,&asf); + TreeOctNode::ProcessFixedDepthNodeAdjacentNodes (fData.depth, sNodes.treeNodes[i], 1, sNodes.treeNodes[j], 2 * width - 1, depth, &asf); } // Get the associated vector SubValues.Resize (asf.adjacencyCount); for (j = 0; j < asf.adjacencyCount; j++) - SubValues[j] = Values[asf.adjacencies[j]-sNodes.nodeCount[depth]]; + SubValues[j] = Values[asf.adjacencies[j] - sNodes.nodeCount[depth]]; SubSolution.Resize (asf.adjacencyCount); for (j = 0; j < asf.adjacencyCount; j++) SubSolution[j] = sNodes.treeNodes[asf.adjacencies[j]]->nodeData.value; // Get the associated matrix SparseSymmetricMatrix::AllocatorMatrixEntry.rollBack (); - GetRestrictedFixedDepthLaplacian (matrix,depth,asf.adjacencies,asf.adjacencyCount,sNodes.treeNodes[i],myRadius,sNodes); + GetRestrictedFixedDepthLaplacian (matrix, depth, asf.adjacencies, asf.adjacencyCount, sNodes.treeNodes[i], myRadius, sNodes); // Solve the matrix - iter+= SparseSymmetricMatrix::Solve (matrix,SubValues,int (pow (matrix.rows,ITERATION_POWER)),SubSolution,double (EPSILON),0); + iter += SparseSymmetricMatrix::Solve (matrix, SubValues, int (pow (matrix.rows, ITERATION_POWER)), SubSolution, double (EPSILON), 0); LaplacianProjectionFunction lpf; lpf.ot = this; @@ -1022,17 +1081,17 @@ namespace pcl { for (j = 0; j < asf.adjacencyCount; j++) { temp = sNodes.treeNodes[asf.adjacencies[j]]; - while (temp->depth ()>sNodes.treeNodes[i]->depth ()) + while (temp->depth () > sNodes.treeNodes[i]->depth ()) temp = temp->parent; - if (temp->nodeData.nodeIndex>= sNodes.treeNodes[i]->nodeData.nodeIndex) + if (temp->nodeData.nodeIndex >= sNodes.treeNodes[i]->nodeData.nodeIndex) sNodes.treeNodes[asf.adjacencies[j]]->nodeData.value = Real (SubSolution[j]); } // Update the values in the next depth int x1, x2, y1, y2, z1, z2; - if (depth < sNodes.maxDepth-1) + if (depth < sNodes.maxDepth - 1) { - int idx1,idx2; - TreeOctNode *node1,*node2; + int idx1, idx2; + TreeOctNode *node1, *node2; // First pass: idx2 is the solution coefficient propogated for (j = 0; j < matrix.rows; j++) { @@ -1052,7 +1111,7 @@ namespace pcl { temp = node2; while (temp->depth () > d) temp = temp->parent; - if (temp!= sNodes.treeNodes[i]) + if (temp != sNodes.treeNodes[i]) continue; lpf.value = Real (SubSolution[matrix.m_ppElements[j][k].N]); x2 = int (node2->off[0]); @@ -1061,13 +1120,13 @@ namespace pcl { lpf.index[0] = x2; lpf.index[1] = y2; lpf.index[2] = z2; - dx = Real (x2-x1)/ (1<processNodeNodes (node2,&lpf,0); + node1->processNodeNodes (node2, &lpf, 0); else - TreeOctNode::ProcessNodeAdjacentNodes (fData.depth,node2,width,node1,width,&lpf,0); + TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, node2, width, node1, width, &lpf, 0); } } // Second pass: idx1 is the solution coefficient propogated @@ -1078,7 +1137,7 @@ namespace pcl { temp = node1; while (temp->depth () > d) temp = temp->parent; - if (temp!= sNodes.treeNodes[i]) + if (temp != sNodes.treeNodes[i]) continue; x1 = int (node1->off[0]); y1 = int (node1->off[1]); @@ -1095,18 +1154,18 @@ namespace pcl { if (!node2->children) continue; - if (idx1!= idx2) + if (idx1 != idx2) { x2 = int (node2->off[0]); y2 = int (node2->off[1]); z2 = int (node2->off[2]); - dx = Real (x1-x2)/ (1<processNodeNodes (node1,&lpf,0); + node2->processNodeNodes (node1, &lpf, 0); else - TreeOctNode::ProcessNodeAdjacentNodes (fData.depth,node1,width,node2,width,&lpf,0); + TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, node1, width, node2, width, &lpf, 0); } } } @@ -1118,23 +1177,27 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::HasNormals (TreeOctNode* node,const Real& epsilon) + int Octree + ::HasNormals (TreeOctNode* node, const Real& epsilon) { int hasNormals = 0; if (node->nodeData.nodeIndex >= 0 && Length ((*normals)[node->nodeData.nodeIndex]) > epsilon) hasNormals = 1; if (node->children) for (int i = 0; i < Cube::CORNERS && !hasNormals; i++) - hasNormals |= HasNormals (&node->children[i],epsilon); + hasNormals |= HasNormals (&node->children[i], epsilon); return hasNormals; } ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::ClipTree () + void Octree + ::ClipTree () { TreeOctNode* temp; temp = tree.nextNode (); @@ -1144,7 +1207,7 @@ namespace pcl { { int hasNormals = 0; for (int i = 0; i < Cube::CORNERS && !hasNormals; i++) - hasNormals = HasNormals (&temp->children[i],EPSILON); + hasNormals = HasNormals (&temp->children[i], EPSILON); if (!hasNormals) temp->children = NULL; } @@ -1154,8 +1217,10 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::SetLaplacianWeights () + void Octree + ::SetLaplacianWeights () { TreeOctNode* temp; @@ -1165,17 +1230,17 @@ namespace pcl { temp = tree.nextNode (); while (temp) { - if (temp->nodeData.nodeIndex<0 || Length ( (*normals)[temp->nodeData.nodeIndex])<= EPSILON) + if (temp->nodeData.nodeIndex < 0 || Length ((*normals)[temp->nodeData.nodeIndex]) <= EPSILON) { temp = tree.nextNode (temp); continue; } - int d = temp->depth (); - df.normal = (*normals)[temp->nodeData.nodeIndex]; + //int d = temp->depth (); + df.normal = (*normals)[temp->nodeData.nodeIndex]; df.index[0] = int (temp->off[0]); df.index[1] = int (temp->off[1]); df.index[2] = int (temp->off[2]); - TreeOctNode::ProcessNodeAdjacentNodes (fData.depth,temp,width,&tree,width,&df); + TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, temp, width, &tree, width, &df); temp = tree.nextNode (temp); } fData.clearDotTables (fData.D_DOT_FLAG); @@ -1185,7 +1250,7 @@ namespace pcl { if (temp->nodeData.nodeIndex < 0) temp->nodeData.centerWeightContribution = 0; else - temp->nodeData.centerWeightContribution = Real (Length ( (*normals)[temp->nodeData.nodeIndex])); + temp->nodeData.centerWeightContribution = Real (Length ((*normals)[temp->nodeData.nodeIndex])); temp = tree.nextNode (temp); } @@ -1195,51 +1260,61 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::DivergenceFunction::Function (TreeOctNode* node1, const TreeOctNode* node2) + void Octree + ::DivergenceFunction::Function (TreeOctNode* node1, const TreeOctNode* /*node2*/) { Point3D n = normal; - if (FunctionData::SymmetricIndex (index[0],int (node1->off[0]),scratch[0])) + if (FunctionData::SymmetricIndex (index[0], int (node1->off[0]), scratch[0])) n.coords[0] = -n.coords[0]; - if (FunctionData::SymmetricIndex (index[1],int (node1->off[1]),scratch[1])) + if (FunctionData::SymmetricIndex (index[1], int (node1->off[1]), scratch[1])) n.coords[1] = -n.coords[1]; - if (FunctionData::SymmetricIndex (index[2],int (node1->off[2]),scratch[2])) + if (FunctionData::SymmetricIndex (index[2], int (node1->off[2]), scratch[2])) n.coords[2] = -n.coords[2]; - double dot = ot->fData.dotTable[scratch[0]]*ot->fData.dotTable[scratch[1]]*ot->fData.dotTable[scratch[2]]; - node1->nodeData.value+= Real (dot* (ot->fData.dDotTable[scratch[0]]*n.coords[0]+ot->fData.dDotTable[scratch[1]]*n.coords[1]+ot->fData.dDotTable[scratch[2]]*n.coords[2])); + double dot = ot->fData.dotTable[scratch[0]] * ot->fData.dotTable[scratch[1]] * ot->fData.dotTable[scratch[2]]; + node1->nodeData.value += Real (dot * (ot->fData.dDotTable[scratch[0]] * n.coords[0] + ot->fData.dDotTable[scratch[1]] * n.coords[1] + ot->fData.dDotTable[scratch[2]] * n.coords[2])); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::LaplacianProjectionFunction::Function (TreeOctNode* node1, const TreeOctNode* node2) + void Octree + ::LaplacianProjectionFunction::Function (TreeOctNode* node1, const TreeOctNode* /*node2*/) { - scratch[0] = FunctionData::SymmetricIndex (index[0],int (node1->off[0])); - scratch[1] = FunctionData::SymmetricIndex (index[1],int (node1->off[1])); - scratch[2] = FunctionData::SymmetricIndex (index[2],int (node1->off[2])); - node1->nodeData.value-= Real (ot->GetLaplacian (scratch)*value); + scratch[0] = FunctionData::SymmetricIndex (index[0], int (node1->off[0])); + scratch[1] = FunctionData::SymmetricIndex (index[1], int (node1->off[1])); + scratch[2] = FunctionData::SymmetricIndex (index[2], int (node1->off[2])); + node1->nodeData.value -= Real (ot->GetLaplacian (scratch) * value); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::AdjacencyCountFunction::Function (const TreeOctNode* node1, const TreeOctNode* node2) + void Octree + ::AdjacencyCountFunction::Function (const TreeOctNode* /*node1*/, const TreeOctNode* /*node2*/) { adjacencyCount++; } ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::AdjacencySetFunction::Function (const TreeOctNode* node1, const TreeOctNode* node2) + void Octree + ::AdjacencySetFunction::Function (const TreeOctNode* node1, const TreeOctNode* /*node2*/) { adjacencies[adjacencyCount++] = node1->nodeData.nodeIndex; } ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::RefineFunction::Function (TreeOctNode* node1, const TreeOctNode* node2) + void Octree + ::RefineFunction::Function (TreeOctNode* node1, const TreeOctNode* /*node2*/) { if (!node1->children && node1->depth () < depth) node1->initChildren (); @@ -1247,42 +1322,44 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::FaceEdgesFunction::Function (const TreeOctNode* node1, const TreeOctNode* node2) + void Octree + ::FaceEdgesFunction::Function (const TreeOctNode* node1, const TreeOctNode* /*node2*/) { if (!node1->children && MarchingCubes::HasRoots (node1->nodeData.mcIndex)) { - RootInfo ri1,ri2; - hash_map >::iterator iter; - int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES]; - int count = MarchingCubes::AddTriangleIndices (node1->nodeData.mcIndex,isoTri); + RootInfo ri1, ri2; + hash_map >::iterator iter; + int isoTri[DIMENSION * MarchingCubes::MAX_TRIANGLES]; + int count = MarchingCubes::AddTriangleIndices (node1->nodeData.mcIndex, isoTri); for (int j = 0; j < count; j++) { for (int k = 0; k < 3; k++) { - if (fIndex == Cube::FaceAdjacentToEdges (isoTri[j*3+k],isoTri[j*3+ ( (k+1)%3)])) + if (fIndex == Cube::FaceAdjacentToEdges (isoTri[j * 3 + k], isoTri[j * 3 + ((k + 1) % 3)])) { - if (GetRootIndex (node1,isoTri[j*3+k],maxDepth,ri1) && GetRootIndex (node1,isoTri[j*3+ ( (k+1)%3)],maxDepth,ri2)) + if (GetRootIndex (node1, isoTri[j * 3 + k], maxDepth, ri1) && GetRootIndex (node1, isoTri[j * 3 + ((k + 1) % 3)], maxDepth, ri2)) { - edges->push_back (std::pair (ri2.key,ri1.key)); + edges->push_back (std::pair (ri2.key, ri1.key)); iter = vertexCount->find (ri1.key); if (iter == vertexCount->end ()) { - (*vertexCount)[ri1.key].first = ri1; - (*vertexCount)[ri1.key].second = 0; + (*vertexCount)[ri1.key].first = ri1; + (*vertexCount)[ri1.key].second = 0; } iter = vertexCount->find (ri2.key); if (iter == vertexCount->end ()) { - (*vertexCount)[ri2.key].first = ri2; - (*vertexCount)[ri2.key].second = 0; + (*vertexCount)[ri2.key].first = ri2; + (*vertexCount)[ri2.key].second = 0; } - (*vertexCount)[ri1.key].second--; - (*vertexCount)[ri2.key].second++; + (*vertexCount)[ri1.key].second--; + (*vertexCount)[ri2.key].second++; } else - PCL_ERROR ("Bad Edge 1: %d %d\n",ri1.key,ri2.key); + PCL_ERROR ("Bad Edge 1: %d %d\n", ri1.key, ri2.key); } } } @@ -1291,71 +1368,77 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::PointIndexValueFunction::Function (const TreeOctNode* node) + void Octree + ::PointIndexValueFunction::Function (const TreeOctNode* node) { int idx[DIMENSION]; - idx[0] = index[0]+int (node->off[0]); - idx[1] = index[1]+int (node->off[1]); - idx[2] = index[2]+int (node->off[2]); - value+= node->nodeData.value* Real ( valueTables[idx[0]]* valueTables[idx[1]]* valueTables[idx[2]]); + idx[0] = index[0] + int (node->off[0]); + idx[1] = index[1] + int (node->off[1]); + idx[2] = index[2] + int (node->off[2]); + value += node->nodeData.value * Real (valueTables[idx[0]] * valueTables[idx[1]] * valueTables[idx[2]]); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::PointIndexValueAndNormalFunction::Function (const TreeOctNode* node) + void Octree + ::PointIndexValueAndNormalFunction::Function (const TreeOctNode* node) { int idx[DIMENSION]; - idx[0] = index[0]+int (node->off[0]); - idx[1] = index[1]+int (node->off[1]); - idx[2] = index[2]+int (node->off[2]); - value += node->nodeData.value* Real ( valueTables[idx[0]]* valueTables[idx[1]]* valueTables[idx[2]]); - normal.coords[0] += node->nodeData.value* Real (dValueTables[idx[0]]* valueTables[idx[1]]* valueTables[idx[2]]); - normal.coords[1] += node->nodeData.value* Real ( valueTables[idx[0]]*dValueTables[idx[1]]* valueTables[idx[2]]); - normal.coords[2] += node->nodeData.value* Real ( valueTables[idx[0]]* valueTables[idx[1]]*dValueTables[idx[2]]); + idx[0] = index[0] + int (node->off[0]); + idx[1] = index[1] + int (node->off[1]); + idx[2] = index[2] + int (node->off[2]); + value += node->nodeData.value * Real (valueTables[idx[0]] * valueTables[idx[1]] * valueTables[idx[2]]); + normal.coords[0] += node->nodeData.value * Real (dValueTables[idx[0]] * valueTables[idx[1]] * valueTables[idx[2]]); + normal.coords[1] += node->nodeData.value * Real (valueTables[idx[0]] * dValueTables[idx[1]] * valueTables[idx[2]]); + normal.coords[2] += node->nodeData.value * Real (valueTables[idx[0]] * valueTables[idx[1]] * dValueTables[idx[2]]); } ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::LaplacianMatrixFunction::Function (const TreeOctNode* node1,const TreeOctNode* node2) + int Octree + ::LaplacianMatrixFunction::Function (const TreeOctNode* node1, const TreeOctNode* node2) { Real temp; int d1 = int (node1->d); - int x1,y1,z1; + int x1, y1, z1; x1 = int (node1->off[0]); y1 = int (node1->off[1]); z1 = int (node1->off[2]); - int dDepth = d2-d1; + int dDepth = d2 - d1; int d; - d = (x2>>dDepth)-x1; + d = (x2 >> dDepth) - x1; if (d < 0) return 0; if (!dDepth) { if (!d) { - d = y2-y1; + d = y2 - y1; if (d < 0) return 0; else if (!d) { - d = z2-z1; + d = z2 - z1; if (d < 0) return 0; } } - scratch[0] = FunctionData::SymmetricIndex (index[0],x1); - scratch[1] = FunctionData::SymmetricIndex (index[1],y1); - scratch[2] = FunctionData::SymmetricIndex (index[2],z1); + scratch[0] = FunctionData::SymmetricIndex (index[0], x1); + scratch[1] = FunctionData::SymmetricIndex (index[1], y1); + scratch[2] = FunctionData::SymmetricIndex (index[2], z1); temp = ot->GetLaplacian (scratch); if (node1 == node2) - temp/= 2; + temp /= 2; if (fabs (temp) > EPSILON) { rowElements[elementCount].Value = temp; - rowElements[elementCount].N = node1->nodeData.nodeIndex-offset; + rowElements[elementCount].N = node1->nodeData.nodeIndex - offset; elementCount++; } return 0; @@ -1365,15 +1448,17 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::RestrictedLaplacianMatrixFunction::Function (const TreeOctNode* node1, const TreeOctNode* node2) + int Octree + ::RestrictedLaplacianMatrixFunction::Function (const TreeOctNode* node1, const TreeOctNode* node2) { - int d1,d2,off1[3],off2[3]; - node1->depthAndOffset (d1,off1); - node2->depthAndOffset (d2,off2); - int dDepth = d2-d1; + int d1, d2, off1[3], off2[3]; + node1->depthAndOffset (d1, off1); + node2->depthAndOffset (d2, off2); + int dDepth = d2 - d1; int d; - d = (off2[0]>>dDepth)-off1[0]; + d = (off2[0] >> dDepth) - off1[0]; if (d < 0) return 0; @@ -1381,25 +1466,25 @@ namespace pcl { { if (!d) { - d = off2[1]-off1[1]; + d = off2[1] - off1[1]; if (d < 0) return 0; else if (!d) { - d = off2[2]-off1[2]; + d = off2[2] - off1[2]; if (d < 0) return 0; } } // Since we are getting the restricted matrix, we don't want to propogate out to terms that don't contribute... - if (!TreeOctNode::Overlap2 (depth,offset,0.5,d1,off1,radius)) + if (!TreeOctNode::Overlap2 (depth, offset, 0.5, d1, off1, radius)) return 0; - scratch[0] = FunctionData::SymmetricIndex (index[0],BinaryNode::Index (d1,off1[0])); - scratch[1] = FunctionData::SymmetricIndex (index[1],BinaryNode::Index (d1,off1[1])); - scratch[2] = FunctionData::SymmetricIndex (index[2],BinaryNode::Index (d1,off1[2])); + scratch[0] = FunctionData::SymmetricIndex (index[0], BinaryNode::Index (d1, off1[0])); + scratch[1] = FunctionData::SymmetricIndex (index[1], BinaryNode::Index (d1, off1[1])); + scratch[2] = FunctionData::SymmetricIndex (index[2], BinaryNode::Index (d1, off1[2])); Real temp = ot->GetLaplacian (scratch); if (node1 == node2) - temp/= 2; + temp /= 2; if (fabs (temp) > EPSILON) { rowElements[elementCount].Value = temp; @@ -1413,29 +1498,31 @@ namespace pcl { ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::GetMCIsoTriangles (const Real& isoValue, - CoredMeshData* mesh, - const int& fullDepthIso, - const int& nonLinearFit, - bool addBarycenter, - bool polygonMesh) - { - double t; + void Octree + ::GetMCIsoTriangles (const Real& isoValue, + CoredMeshData* mesh, + const int& fullDepthIso, + const int& nonLinearFit, + bool addBarycenter, + bool polygonMesh) + { + //double t; TreeOctNode* temp; - hash_map roots; - hash_map > > *normalHash = new hash_map > > (); + hash_map roots; + hash_map > > *normalHash = new hash_map > > (); - SetIsoSurfaceCorners (isoValue,0,fullDepthIso); + SetIsoSurfaceCorners (isoValue, 0, fullDepthIso); // At the point all of the corner values have been set and all nodes are valid. Now it's just a matter // of running marching cubes. - fData.setValueTables (fData.VALUE_FLAG | fData.D_VALUE_FLAG,0,postNormalSmooth); + fData.setValueTables (fData.VALUE_FLAG | fData.D_VALUE_FLAG, 0, postNormalSmooth); temp = tree.nextLeaf (); while (temp) { - SetMCRootPositions (temp,0,isoValue,roots,NULL,*normalHash,NULL,NULL,mesh,nonLinearFit); + SetMCRootPositions (temp, 0, isoValue, roots, NULL, *normalHash, NULL, NULL, mesh, nonLinearFit); temp = tree.nextLeaf (temp); } @@ -1447,59 +1534,61 @@ namespace pcl { temp = tree.nextLeaf (); while (temp) { - GetMCIsoTriangles (temp,mesh,roots,NULL,NULL,0,0 , addBarycenter , polygonMesh ); + GetMCIsoTriangles (temp, mesh, roots, NULL, NULL, 0, 0, addBarycenter, polygonMesh); temp = tree.nextLeaf (temp); } } ////////////////////////////////////////////////////////////////////////////////////////////// + template - void Octree::GetMCIsoTriangles (const Real& isoValue, - const int& subdivideDepth, - CoredMeshData* mesh, - const int& fullDepthIso, - const int& nonLinearFit, - bool addBarycenter, - bool polygonMesh) + void Octree + ::GetMCIsoTriangles (const Real& isoValue, + const int& subdivideDepth, + CoredMeshData* mesh, + const int& fullDepthIso, + const int& nonLinearFit, + bool addBarycenter, + bool polygonMesh) { TreeOctNode* temp; - hash_map boundaryRoots,*interiorRoots; - hash_map > > *boundaryNormalHash,*interiorNormalHash; + hash_map boundaryRoots, *interiorRoots; + hash_map > > *boundaryNormalHash, *interiorNormalHash; std::vector >* interiorPoints; int sDepth; if (subdivideDepth <= 0) sDepth = 0; else - sDepth = fData.depth-subdivideDepth; + sDepth = fData.depth - subdivideDepth; if (sDepth < 0) sDepth = 0; - SetIsoSurfaceCorners (isoValue,sDepth,fullDepthIso); + SetIsoSurfaceCorners (isoValue, sDepth, fullDepthIso); // At this point all of the corner values have been set and all nodes are valid. Now it's just a matter // of running marching cubes. - boundaryNormalHash = new hash_map > > (); + boundaryNormalHash = new hash_map > > (); int offSet = 0; SortedTreeNodes sNodes; - sNodes.set (tree,0); - fData.setValueTables (fData.VALUE_FLAG | fData.D_VALUE_FLAG,0,postNormalSmooth); + sNodes.set (tree, 0); + fData.setValueTables (fData.VALUE_FLAG | fData.D_VALUE_FLAG, 0, postNormalSmooth); // Set the root positions for all leaf nodes below the subdivide threshold - SetBoundaryMCRootPositions (sDepth,isoValue,boundaryRoots,*boundaryNormalHash,mesh,nonLinearFit); + SetBoundaryMCRootPositions (sDepth, isoValue, boundaryRoots, *boundaryNormalHash, mesh, nonLinearFit); - for (int i = sNodes.nodeCount[sDepth]; i < sNodes.nodeCount[sDepth+1]; i++) + for (int i = sNodes.nodeCount[sDepth]; i < sNodes.nodeCount[sDepth + 1]; i++) { - interiorRoots = new hash_map (); - interiorNormalHash = new hash_map > > (); + interiorRoots = new hash_map (); + interiorNormalHash = new hash_map > > (); interiorPoints = new std::vector > (); temp = sNodes.treeNodes[i]->nextLeaf (); while (temp) { if (MarchingCubes::HasRoots (temp->nodeData.mcIndex)) - SetMCRootPositions (temp,sDepth,isoValue,boundaryRoots,interiorRoots,*boundaryNormalHash,interiorNormalHash,interiorPoints,mesh,nonLinearFit); + SetMCRootPositions (temp, sDepth, isoValue, boundaryRoots, interiorRoots, *boundaryNormalHash, interiorNormalHash, interiorPoints, mesh, nonLinearFit); temp = sNodes.treeNodes[i]->nextLeaf (temp); } delete interiorNormalHash; @@ -1507,7 +1596,7 @@ namespace pcl { temp = sNodes.treeNodes[i]->nextLeaf (); while (temp) { - GetMCIsoTriangles (temp,mesh,boundaryRoots,interiorRoots,interiorPoints,offSet,sDepth , addBarycenter , polygonMesh ); + GetMCIsoTriangles (temp, mesh, boundaryRoots, interiorRoots, interiorPoints, offSet, sDepth, addBarycenter, polygonMesh); temp = sNodes.treeNodes[i]->nextLeaf (temp); } delete interiorRoots; @@ -1520,95 +1609,125 @@ namespace pcl { while (temp) { if (temp->depth () < sDepth) - GetMCIsoTriangles (temp,mesh,boundaryRoots,NULL,NULL,0,0 , addBarycenter , polygonMesh ); + GetMCIsoTriangles (temp, mesh, boundaryRoots, NULL, NULL, 0, 0, addBarycenter, polygonMesh); temp = tree.nextLeaf (temp); } } ////////////////////////////////////////////////////////////////////////////////////////////// + template - Real Octree::getCenterValue (const TreeOctNode* node){ + Real Octree + ::getCenterValue (const TreeOctNode* node) + { int idx[3]; Real value = 0; neighborKey2.getNeighbors (node); - VertexData::CenterIndex (node,fData.depth,idx); - idx[0]*= fData.res; - idx[1]*= fData.res; - idx[2]*= fData.res; - for (int i = 0;i<= node->depth ();i++){ - for (int j = 0;j<3;j++){ - for (int k = 0;k<3;k++){ - for (int l = 0;l<3;l++){ + VertexData::CenterIndex (node, fData.depth, idx); + idx[0] *= fData.res; + idx[1] *= fData.res; + idx[2] *= fData.res; + for (int i = 0; i <= node->depth (); i++) + { + for (int j = 0; j < 3; j++) + { + for (int k = 0; k < 3; k++) + { + for (int l = 0; l < 3; l++) + { const TreeOctNode* n = neighborKey2.neighbors[i].neighbors[j][k][l]; - if (n){ + if (n) + { Real temp = n->nodeData.value; - value+= temp*Real ( - fData.valueTables[idx[0]+int (n->off[0])]* - fData.valueTables[idx[1]+int (n->off[1])]* - fData.valueTables[idx[2]+int (n->off[2])]); + value += temp * Real ( + fData.valueTables[idx[0] + int (n->off[0])] * + fData.valueTables[idx[1] + int (n->off[1])] * + fData.valueTables[idx[2] + int (n->off[2])]); } } } } } - if (node->children){ - for (int i = 0;ichildren) + { + for (int i = 0; i < Cube::CORNERS; i++) + { int ii = Cube::AntipodalCornerIndex (i); const TreeOctNode* n = &node->children[i]; - while (1){ - value+= n->nodeData.value*Real ( - fData.valueTables[idx[0]+int (n->off[0])]* - fData.valueTables[idx[1]+int (n->off[1])]* - fData.valueTables[idx[2]+int (n->off[2])]); - if (n->children){n = &n->children[ii];} - else{break;} + while (1) + { + value += n->nodeData.value * Real ( + fData.valueTables[idx[0] + int (n->off[0])] * + fData.valueTables[idx[1] + int (n->off[1])] * + fData.valueTables[idx[2] + int (n->off[2])]); + if (n->children) + { + n = &n->children[ii]; + } + else + { + break; + } } } } return value; } + template - Real Octree::getCornerValue (const TreeOctNode* node,const int& corner){ + Real Octree + ::getCornerValue (const TreeOctNode* node, const int& corner) + { int idx[3]; Real value = 0; neighborKey2.getNeighbors (node); - VertexData::CornerIndex (node,corner,fData.depth,idx); - idx[0]*= fData.res; - idx[1]*= fData.res; - idx[2]*= fData.res; - for (int i = 0;i<= node->depth ();i++){ - for (int j = 0;j<3;j++){ - for (int k = 0;k<3;k++){ - for (int l = 0;l<3;l++){ + VertexData::CornerIndex (node, corner, fData.depth, idx); + idx[0] *= fData.res; + idx[1] *= fData.res; + idx[2] *= fData.res; + for (int i = 0; i <= node->depth (); i++) + { + for (int j = 0; j < 3; j++) + { + for (int k = 0; k < 3; k++) + { + for (int l = 0; l < 3; l++) + { const TreeOctNode* n = neighborKey2.neighbors[i].neighbors[j][k][l]; - if (n){ + if (n) + { Real temp = n->nodeData.value; - value+= temp*Real ( - fData.valueTables[idx[0]+int (n->off[0])]* - fData.valueTables[idx[1]+int (n->off[1])]* - fData.valueTables[idx[2]+int (n->off[2])]); + value += temp * Real ( + fData.valueTables[idx[0] + int (n->off[0])] * + fData.valueTables[idx[1] + int (n->off[1])] * + fData.valueTables[idx[2] + int (n->off[2])]); } } } } } - int x,y,z,d = node->depth (); - Cube::FactorCornerIndex (corner,x,y,z); - for (int i = 0;i<2;i++){ - for (int j = 0;j<2;j++){ - for (int k = 0;k<2;k++){ - const TreeOctNode* n = neighborKey2.neighbors[d].neighbors[x+i][y+j][z+k]; - if (n){ - int ii = Cube::AntipodalCornerIndex (Cube::CornerIndex (i,j,k)); - while (n->children){ + int x, y, z, d = node->depth (); + Cube::FactorCornerIndex (corner, x, y, z); + for (int i = 0; i < 2; i++) + { + for (int j = 0; j < 2; j++) + { + for (int k = 0; k < 2; k++) + { + const TreeOctNode* n = neighborKey2.neighbors[d].neighbors[x + i][y + j][z + k]; + if (n) + { + int ii = Cube::AntipodalCornerIndex (Cube::CornerIndex (i, j, k)); + while (n->children) + { n = &n->children[ii]; - value+= n->nodeData.value*Real ( - fData.valueTables[idx[0]+int (n->off[0])]* - fData.valueTables[idx[1]+int (n->off[1])]* - fData.valueTables[idx[2]+int (n->off[2])]); + value += n->nodeData.value * Real ( + fData.valueTables[idx[0] + int (n->off[0])] * + fData.valueTables[idx[1] + int (n->off[1])] * + fData.valueTables[idx[2] + int (n->off[2])]); } } } @@ -1616,123 +1735,149 @@ namespace pcl { } return value; } + template - void Octree::getCornerValueAndNormal (const TreeOctNode* node,const int& corner,Real& value,Point3D& normal){ - int idx[3],index[3]; + void Octree + ::getCornerValueAndNormal (const TreeOctNode* node, const int& corner, Real& value, Point3D& normal) + { + int idx[3], index[3]; value = normal.coords[0] = normal.coords[1] = normal.coords[2] = 0; neighborKey2.getNeighbors (node); - VertexData::CornerIndex (node,corner,fData.depth,idx); - idx[0]*= fData.res; - idx[1]*= fData.res; - idx[2]*= fData.res; - for (int i = 0;i<= node->depth ();i++){ - for (int j = 0;j<3;j++){ - for (int k = 0;k<3;k++){ - for (int l = 0;l<3;l++){ + VertexData::CornerIndex (node, corner, fData.depth, idx); + idx[0] *= fData.res; + idx[1] *= fData.res; + idx[2] *= fData.res; + for (int i = 0; i <= node->depth (); i++) + { + for (int j = 0; j < 3; j++) + { + for (int k = 0; k < 3; k++) + { + for (int l = 0; l < 3; l++) + { const TreeOctNode* n = neighborKey2.neighbors[i].neighbors[j][k][l]; - if (n){ + if (n) + { Real temp = n->nodeData.value; - index[0] = idx[0]+int (n->off[0]); - index[1] = idx[1]+int (n->off[1]); - index[2] = idx[2]+int (n->off[2]); - value+= temp*Real (fData.valueTables[index[0]]*fData.valueTables[index[1]]*fData.valueTables[index[2]]); - normal.coords[0]+= temp*Real (fData.dValueTables[index[0]]* fData.valueTables[index[1]]* fData.valueTables[index[2]]); - normal.coords[1]+= temp*Real ( fData.valueTables[index[0]]*fData.dValueTables[index[1]]* fData.valueTables[index[2]]); - normal.coords[2]+= temp*Real ( fData.valueTables[index[0]]* fData.valueTables[index[1]]*fData.dValueTables[index[2]]); + index[0] = idx[0] + int (n->off[0]); + index[1] = idx[1] + int (n->off[1]); + index[2] = idx[2] + int (n->off[2]); + value += temp * Real (fData.valueTables[index[0]] * fData.valueTables[index[1]] * fData.valueTables[index[2]]); + normal.coords[0] += temp * Real (fData.dValueTables[index[0]] * fData.valueTables[index[1]] * fData.valueTables[index[2]]); + normal.coords[1] += temp * Real (fData.valueTables[index[0]] * fData.dValueTables[index[1]] * fData.valueTables[index[2]]); + normal.coords[2] += temp * Real (fData.valueTables[index[0]] * fData.valueTables[index[1]] * fData.dValueTables[index[2]]); } } } } } - int x,y,z,d = node->depth (); - Cube::FactorCornerIndex (corner,x,y,z); - for (int i = 0;i<2;i++){ - for (int j = 0;j<2;j++){ - for (int k = 0;k<2;k++){ - const TreeOctNode* n = neighborKey2.neighbors[d].neighbors[x+i][y+j][z+k]; - if (n){ - int ii = Cube::AntipodalCornerIndex (Cube::CornerIndex (i,j,k)); - while (n->children){ + int x, y, z, d = node->depth (); + Cube::FactorCornerIndex (corner, x, y, z); + for (int i = 0; i < 2; i++) + { + for (int j = 0; j < 2; j++) + { + for (int k = 0; k < 2; k++) + { + const TreeOctNode* n = neighborKey2.neighbors[d].neighbors[x + i][y + j][z + k]; + if (n) + { + int ii = Cube::AntipodalCornerIndex (Cube::CornerIndex (i, j, k)); + while (n->children) + { n = &n->children[ii]; Real temp = n->nodeData.value; - index[0] = idx[0]+int (n->off[0]); - index[1] = idx[1]+int (n->off[1]); - index[2] = idx[2]+int (n->off[2]); - value+= temp*Real (fData.valueTables[index[0]]*fData.valueTables[index[1]]*fData.valueTables[index[2]]); - normal.coords[0]+= temp*Real (fData.dValueTables[index[0]]* fData.valueTables[index[1]]* fData.valueTables[index[2]]); - normal.coords[1]+= temp*Real ( fData.valueTables[index[0]]*fData.dValueTables[index[1]]* fData.valueTables[index[2]]); - normal.coords[2]+= temp*Real ( fData.valueTables[index[0]]* fData.valueTables[index[1]]*fData.dValueTables[index[2]]); + index[0] = idx[0] + int (n->off[0]); + index[1] = idx[1] + int (n->off[1]); + index[2] = idx[2] + int (n->off[2]); + value += temp * Real (fData.valueTables[index[0]] * fData.valueTables[index[1]] * fData.valueTables[index[2]]); + normal.coords[0] += temp * Real (fData.dValueTables[index[0]] * fData.valueTables[index[1]] * fData.valueTables[index[2]]); + normal.coords[1] += temp * Real (fData.valueTables[index[0]] * fData.dValueTables[index[1]] * fData.valueTables[index[2]]); + normal.coords[2] += temp * Real (fData.valueTables[index[0]] * fData.valueTables[index[1]] * fData.dValueTables[index[2]]); } } } } } } + template - Real Octree::GetIsoValue (){ - if (this->width<= 3){ + Real Octree + ::GetIsoValue () + { + if (this->width <= 3) + { TreeOctNode* temp; - Real isoValue,weightSum,w; + Real isoValue, weightSum, w; neighborKey2.set (fData.depth); - fData.setValueTables (fData.VALUE_FLAG,0); + fData.setValueTables (fData.VALUE_FLAG, 0); isoValue = weightSum = 0; temp = tree.nextNode (); - while (temp){ + while (temp) + { w = temp->nodeData.centerWeightContribution; - if (w>EPSILON){ - isoValue+= getCenterValue (temp)*w; - weightSum+= w; + if (w > EPSILON) + { + isoValue += getCenterValue (temp) * w; + weightSum += w; } temp = tree.nextNode (temp); } - return isoValue/weightSum; + return isoValue / weightSum; } - else{ + else + { const TreeOctNode* temp; - Real isoValue,weightSum,w; - Real myRadius; + Real isoValue, weightSum, w; + //Real myRadius; PointIndexValueFunction cf; - fData.setValueTables (fData.VALUE_FLAG,0); + fData.setValueTables (fData.VALUE_FLAG, 0); cf.valueTables = fData.valueTables; cf.res2 = fData.res2; - myRadius = radius; + //myRadius = radius; isoValue = weightSum = 0; temp = tree.nextNode (); - while (temp){ + while (temp) + { w = temp->nodeData.centerWeightContribution; - if (w>EPSILON){ + if (w > EPSILON) + { cf.value = 0; int idx[3]; - VertexData::CenterIndex (temp,fData.depth,idx); - cf.index[0] = idx[0]*fData.res; - cf.index[1] = idx[1]*fData.res; - cf.index[2] = idx[2]*fData.res; - TreeOctNode::ProcessPointAdjacentNodes (fData.depth,idx,&tree,width,&cf); - isoValue+= cf.value*w; - weightSum+= w; + VertexData::CenterIndex (temp, fData.depth, idx); + cf.index[0] = idx[0] * fData.res; + cf.index[1] = idx[1] * fData.res; + cf.index[2] = idx[2] * fData.res; + TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf); + isoValue += cf.value*w; + weightSum += w; } temp = tree.nextNode (temp); } - return isoValue/weightSum; + return isoValue / weightSum; } } + template - void Octree::SetIsoSurfaceCorners (const Real& isoValue,const int& subdivideDepth,const int& fullDepthIso){ - int i,j; - hash_map values; + void Octree + ::SetIsoSurfaceCorners (const Real& isoValue, const int& subdivideDepth, const int& /*fullDepthIso*/) + { + int i, j; + hash_map values; Real cornerValues[Cube::CORNERS]; PointIndexValueFunction cf; TreeOctNode* temp; - int leafCount = tree.leaves (); + //int leafCount = tree.leaves (); long long key; SortedTreeNodes *sNodes = new SortedTreeNodes (); - sNodes->set (tree,0); + sNodes->set (tree, 0); temp = tree.nextNode (); - while (temp){ + while (temp) + { temp->nodeData.mcIndex = 0; temp = tree.nextNode (temp); } @@ -1740,37 +1885,51 @@ namespace pcl { // Start by setting the corner values of all the nodes cf.valueTables = fData.valueTables; cf.res2 = fData.res2; - for (i = 0;inodeCount[subdivideDepth];i++){ + for (i = 0; i < sNodes->nodeCount[subdivideDepth]; i++) + { temp = sNodes->treeNodes[i]; - if (!temp->children){ - for (j = 0;jwidth<= 3){cornerValues[j] = getCornerValue (temp,j);} - else{ + if (!temp->children) + { + for (j = 0; j < Cube::CORNERS; j++) + { + if (this->width <= 3) + { + cornerValues[j] = getCornerValue (temp, j); + } + else + { cf.value = 0; int idx[3]; - VertexData::CornerIndex (temp,j,fData.depth,idx); - cf.index[0] = idx[0]*fData.res; - cf.index[1] = idx[1]*fData.res; - cf.index[2] = idx[2]*fData.res; - TreeOctNode::ProcessPointAdjacentNodes (fData.depth,idx,&tree,width,&cf); + VertexData::CornerIndex (temp, j, fData.depth, idx); + cf.index[0] = idx[0] * fData.res; + cf.index[1] = idx[1] * fData.res; + cf.index[2] = idx[2] * fData.res; + TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf); cornerValues[j] = cf.value; } } - temp->nodeData.mcIndex = MarchingCubes::GetIndex (cornerValues,isoValue); + temp->nodeData.mcIndex = MarchingCubes::GetIndex (cornerValues, isoValue); - if (temp->parent){ + if (temp->parent) + { TreeOctNode* parent = temp->parent; - int c = int (temp-temp->parent->children); - int mcid = temp->nodeData.mcIndex& (1<parent->children); + int mcid = temp->nodeData.mcIndex & (1 << MarchingCubes::cornerMap[c]); - if (mcid){ + if (mcid) + { parent->nodeData.mcIndex |= mcid; - while (1){ - if (parent->parent && (parent-parent->parent->children) == c){ + while (1) + { + if (parent->parent && (parent - parent->parent->children) == c) + { parent->parent->nodeData.mcIndex |= mcid; parent = parent->parent; } - else{break;} + else + { + break; + } } } } @@ -1779,41 +1938,59 @@ namespace pcl { MemoryUsage (); - for (i = sNodes->nodeCount[subdivideDepth];inodeCount[subdivideDepth+1];i++){ + for (i = sNodes->nodeCount[subdivideDepth]; i < sNodes->nodeCount[subdivideDepth + 1]; i++) + { temp = sNodes->treeNodes[i]->nextLeaf (); - while (temp){ - for (j = 0;jwidth<= 3){values[key] = cornerValues[j] = getCornerValue (temp,j);} - else{ + key = VertexData::CornerIndex (temp, j, fData.depth, idx); + cf.index[0] = idx[0] * fData.res; + cf.index[1] = idx[1] * fData.res; + cf.index[2] = idx[2] * fData.res; + if (values.find (key) != values.end ()) + { + cornerValues[j] = values[key]; + } + else + { + if (this->width <= 3) + { + values[key] = cornerValues[j] = getCornerValue (temp, j); + } + else + { cf.value = 0; - TreeOctNode::ProcessPointAdjacentNodes (fData.depth,idx,&tree,width,&cf); + TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf); values[key] = cf.value; cornerValues[j] = cf.value; } } } - temp->nodeData.mcIndex = MarchingCubes::GetIndex (cornerValues,isoValue); + temp->nodeData.mcIndex = MarchingCubes::GetIndex (cornerValues, isoValue); - if (temp->parent){ + if (temp->parent) + { TreeOctNode* parent = temp->parent; - int c = int (temp-temp->parent->children); - int mcid = temp->nodeData.mcIndex& (1<nodeData.mcIndex|= mcid; - while (1){ - if (parent->parent && (parent-parent->parent->children) == c){ - parent->parent->nodeData.mcIndex|= mcid; + int c = int (temp - temp->parent->children); + int mcid = temp->nodeData.mcIndex & (1 << MarchingCubes::cornerMap[c]); + + if (mcid) + { + parent->nodeData.mcIndex |= mcid; + while (1) + { + if (parent->parent && (parent - parent->parent->children) == c) + { + parent->parent->nodeData.mcIndex |= mcid; parent = parent->parent; } - else{break;} + else + { + break; + } } } } @@ -1824,13 +2001,19 @@ namespace pcl { values.clear (); } delete sNodes; - printf ("Memory Usage: %.3f MB\n",float (MemoryUsage ())); + printf ("Memory Usage: %.3f MB\n", float (MemoryUsage ())); - if (subdivideDepth){PreValidate (isoValue,fData.depth,subdivideDepth);} + if (subdivideDepth) + { + PreValidate (isoValue, fData.depth, subdivideDepth); + } } + template - void Octree::Subdivide (TreeOctNode* node,const Real& isoValue,const int& maxDepth){ - int i,j,c[4]; + void Octree + ::Subdivide (TreeOctNode* node, const Real& isoValue, const int& maxDepth) + { + int i, j, c[4]; Real value; int cornerIndex2[Cube::CORNERS]; PointIndexValueFunction cf; @@ -1842,131 +2025,180 @@ namespace pcl { // Now set the corner values for the new children // Copy old corner values - for (i = 0;inodeData.mcIndex& (1<nodeData.mcIndex & (1 << MarchingCubes::cornerMap[i]); + } // 8 of 27 corners set // Set center corner cf.value = 0; int idx[3]; - VertexData::CenterIndex (node,maxDepth,idx); - cf.index[0] = idx[0]*fData.res; - cf.index[1] = idx[1]*fData.res; - cf.index[2] = idx[2]*fData.res; - if (this->width<= 3){value = getCenterValue (node);} - else{ - TreeOctNode::ProcessPointAdjacentNodes (fData.depth,idx,&tree,width,&cf); + VertexData::CenterIndex (node, maxDepth, idx); + cf.index[0] = idx[0] * fData.res; + cf.index[1] = idx[1] * fData.res; + cf.index[2] = idx[2] * fData.res; + if (this->width <= 3) + { + value = getCenterValue (node); + } + else + { + TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf); value = cf.value; } - if (valuechildren[i].nodeData.mcIndex = cornerIndex2[i];} + for (i = 0; i < Cube::CORNERS; i++) + { + node->children[i].nodeData.mcIndex = cornerIndex2[i]; + } } template - int Octree::InteriorFaceRootCount (const TreeOctNode* node,const int &faceIndex,const int& maxDepth){ - int c1,c2,e1,e2,dir,off,cnt = 0; - int corners[Cube::CORNERS/2]; - if (node->children){ - Cube::FaceCorners (faceIndex,corners[0],corners[1],corners[2],corners[3]); - Cube::FactorFaceIndex (faceIndex,dir,off); + int Octree + ::InteriorFaceRootCount (const TreeOctNode* node, const int &faceIndex, const int& maxDepth) + { + int c1, c2, e1, e2, dir, off, cnt = 0; + int corners[Cube::CORNERS / 2]; + if (node->children) + { + Cube::FaceCorners (faceIndex, corners[0], corners[1], corners[2], corners[3]); + Cube::FactorFaceIndex (faceIndex, dir, off); c1 = corners[0]; c2 = corners[3]; - switch (dir){ + switch (dir) + { case 0: - e1 = Cube::EdgeIndex (1,off,1); - e2 = Cube::EdgeIndex (2,off,1); + e1 = Cube::EdgeIndex (1, off, 1); + e2 = Cube::EdgeIndex (2, off, 1); break; case 1: - e1 = Cube::EdgeIndex (0,off,1); - e2 = Cube::EdgeIndex (2,1,off); + e1 = Cube::EdgeIndex (0, off, 1); + e2 = Cube::EdgeIndex (2, 1, off); break; case 2: - e1 = Cube::EdgeIndex (0,1,off); - e2 = Cube::EdgeIndex (1,1,off); + e1 = Cube::EdgeIndex (0, 1, off); + e2 = Cube::EdgeIndex (1, 1, off); break; }; - cnt+= EdgeRootCount (&node->children[c1],e1,maxDepth)+EdgeRootCount (&node->children[c1],e2,maxDepth); - switch (dir){ + cnt += EdgeRootCount (&node->children[c1], e1, maxDepth) + EdgeRootCount (&node->children[c1], e2, maxDepth); + switch (dir) + { case 0: - e1 = Cube::EdgeIndex (1,off,0); - e2 = Cube::EdgeIndex (2,off,0); + e1 = Cube::EdgeIndex (1, off, 0); + e2 = Cube::EdgeIndex (2, off, 0); break; case 1: - e1 = Cube::EdgeIndex (0,off,0); - e2 = Cube::EdgeIndex (2,0,off); + e1 = Cube::EdgeIndex (0, off, 0); + e2 = Cube::EdgeIndex (2, 0, off); break; case 2: - e1 = Cube::EdgeIndex (0,0,off); - e2 = Cube::EdgeIndex (1,0,off); + e1 = Cube::EdgeIndex (0, 0, off); + e2 = Cube::EdgeIndex (1, 0, off); break; }; - cnt+= EdgeRootCount (&node->children[c2],e1,maxDepth)+EdgeRootCount (&node->children[c2],e2,maxDepth); - for (int i = 0;ichildren[corners[i]].children){cnt+= InteriorFaceRootCount (&node->children[corners[i]],faceIndex,maxDepth);}} + cnt += EdgeRootCount (&node->children[c2], e1, maxDepth) + EdgeRootCount (&node->children[c2], e2, maxDepth); + for (int i = 0; i < Cube::CORNERS / 2; i++) + { + if (node->children[corners[i]].children) + { + cnt += InteriorFaceRootCount (&node->children[corners[i]], faceIndex, maxDepth); + } + } } return cnt; } template - int Octree::EdgeRootCount (const TreeOctNode* node,const int& edgeIndex,const int& maxDepth){ - int f1,f2,c1,c2; + int Octree + ::EdgeRootCount (const TreeOctNode* node, const int& edgeIndex, const int& maxDepth) + { + int f1, f2, c1, c2; const TreeOctNode* temp; - Cube::FacesAdjacentToEdge (edgeIndex,f1,f2); + Cube::FacesAdjacentToEdge (edgeIndex, f1, f2); int eIndex; const TreeOctNode* finest = node; eIndex = edgeIndex; - if (node->depth ()depth () < maxDepth) + { temp = node->faceNeighbor (f1); - if (temp && temp->children){ + if (temp && temp->children) + { finest = temp; - eIndex = Cube::FaceReflectEdgeIndex (edgeIndex,f1); + eIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f1); } - else{ + else + { temp = node->faceNeighbor (f2); - if (temp && temp->children){ + if (temp && temp->children) + { finest = temp; - eIndex = Cube::FaceReflectEdgeIndex (edgeIndex,f2); + eIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f2); } - else{ + else + { temp = node->edgeNeighbor (edgeIndex); - if (temp && temp->children){ + if (temp && temp->children) + { finest = temp; eIndex = Cube::EdgeReflectEdgeIndex (edgeIndex); } @@ -1974,72 +2206,116 @@ namespace pcl { } } - Cube::EdgeCorners (eIndex,c1,c2); - if (finest->children){return EdgeRootCount (&finest->children[c1],eIndex,maxDepth)+EdgeRootCount (&finest->children[c2],eIndex,maxDepth);} - else{return MarchingCubes::HasEdgeRoots (finest->nodeData.mcIndex,eIndex);} + Cube::EdgeCorners (eIndex, c1, c2); + if (finest->children) + { + return EdgeRootCount (&finest->children[c1], eIndex, maxDepth) + EdgeRootCount (&finest->children[c2], eIndex, maxDepth); + } + else + { + return MarchingCubes::HasEdgeRoots (finest->nodeData.mcIndex, eIndex); + } } + template - int Octree::IsBoundaryFace (const TreeOctNode* node,const int& faceIndex,const int& subdivideDepth){ - int dir,offset,d,o[3],idx; + int Octree + ::IsBoundaryFace (const TreeOctNode* node, const int& faceIndex, const int& subdivideDepth) + { + int dir, offset, d, o[3], idx; - if (subdivideDepth<0){return 0;} - if (node->d<= subdivideDepth){return 1;} - Cube::FactorFaceIndex (faceIndex,dir,offset); - node->depthAndOffset (d,o); + if (subdivideDepth < 0) + { + return 0; + } + if (node->d <= subdivideDepth) + { + return 1; + } + Cube::FactorFaceIndex (faceIndex, dir, offset); + node->depthAndOffset (d, o); - idx = (int (o[dir])<<1) + (offset<<1); - return ! (idx% (2<< (int (node->d)-subdivideDepth))); + idx = (int (o[dir]) << 1) + (offset << 1); + return !(idx % (2 << (int (node->d) - subdivideDepth))); } + template - int Octree::IsBoundaryEdge (const TreeOctNode* node,const int& edgeIndex,const int& subdivideDepth){ - int dir,x,y; - Cube::FactorEdgeIndex (edgeIndex,dir,x,y); - return IsBoundaryEdge (node,dir,x,y,subdivideDepth); + int Octree + ::IsBoundaryEdge (const TreeOctNode* node, const int& edgeIndex, const int& subdivideDepth) + { + int dir, x, y; + Cube::FactorEdgeIndex (edgeIndex, dir, x, y); + return IsBoundaryEdge (node, dir, x, y, subdivideDepth); } + template - int Octree::IsBoundaryEdge (const TreeOctNode* node,const int& dir,const int& x,const int& y,const int& subdivideDepth){ - int d,o[3],idx1,idx2,mask; + int Octree + ::IsBoundaryEdge (const TreeOctNode* node, const int& dir, const int& x, const int& y, const int& subdivideDepth) + { + int d, o[3], idx1, idx2, mask; - if (subdivideDepth<0){return 0;} - if (node->d<= subdivideDepth){return 1;} - node->depthAndOffset (d,o); + if (subdivideDepth < 0) + { + return 0; + } + if (node->d <= subdivideDepth) + { + return 1; + } + node->depthAndOffset (d, o); - switch (dir){ + // initialize to remove warnings + idx1 = idx2 = 0; + switch (dir) + { case 0: - idx1 = (int (o[1])<<1) + (x<<1); - idx2 = (int (o[2])<<1) + (y<<1); + idx1 = (int (o[1]) << 1) + (x << 1); + idx2 = (int (o[2]) << 1) + (y << 1); break; case 1: - idx1 = (int (o[0])<<1) + (x<<1); - idx2 = (int (o[2])<<1) + (y<<1); + idx1 = (int (o[0]) << 1) + (x << 1); + idx2 = (int (o[2]) << 1) + (y << 1); break; case 2: - idx1 = (int (o[0])<<1) + (x<<1); - idx2 = (int (o[1])<<1) + (y<<1); + idx1 = (int (o[0]) << 1) + (x << 1); + idx2 = (int (o[1]) << 1) + (y << 1); break; } - mask = 2<< (int (node->d)-subdivideDepth); - return ! (idx1% (mask)) || ! (idx2% (mask)); + mask = 2 << (int (node->d) - subdivideDepth); + return !(idx1 % (mask)) || !(idx2 % (mask)); } template - void Octree::PreValidate (TreeOctNode* node,const Real& isoValue,const int& maxDepth,const int& subdivideDepth){ + void Octree + ::PreValidate (TreeOctNode* node, const Real& isoValue, const int& maxDepth, const int& subdivideDepth) + { int sub = 0; - if (node->children){printf ("Bad Pre-Validate\n");} + if (node->children) + { + printf ("Bad Pre-Validate\n"); + } // if (int (node->d)faceNeighbor (i); - if (neighbor && neighbor->children){ - if (IsBoundaryFace (node,i,subdivideDepth) && InteriorFaceRootCount (neighbor,Cube::FaceReflectFaceIndex (i,i),maxDepth)){sub = 1;} + if (neighbor && neighbor->children) + { + if (IsBoundaryFace (node, i, subdivideDepth) && InteriorFaceRootCount (neighbor, Cube::FaceReflectFaceIndex (i, i), maxDepth)) + { + sub = 1; + } } } - if (sub){ - Subdivide (node,isoValue,maxDepth); - for (int i = 0;ifaceNeighbor (i); - while (neighbor && !neighbor->children){ - PreValidate (neighbor,isoValue,maxDepth,subdivideDepth); + while (neighbor && !neighbor->children) + { + PreValidate (neighbor, isoValue, maxDepth, subdivideDepth); neighbor = node->faceNeighbor (i); } } @@ -2048,98 +2324,189 @@ namespace pcl { } template - void Octree::PreValidate (const Real& isoValue,const int& maxDepth,const int& subdivideDepth){ + void Octree + ::PreValidate (const Real& isoValue, const int& maxDepth, const int& subdivideDepth) + { TreeOctNode* temp; temp = tree.nextLeaf (); - while (temp){ - PreValidate (temp,isoValue,maxDepth,subdivideDepth); + while (temp) + { + PreValidate (temp, isoValue, maxDepth, subdivideDepth); temp = tree.nextLeaf (temp); } } + template - void Octree::Validate (TreeOctNode* node,const Real& isoValue,const int& maxDepth,const int& fullDepthIso){ - int i,sub = 0; + void Octree + ::Validate (TreeOctNode* node, const Real& isoValue, const int& maxDepth, const int& fullDepthIso) + { + int i, sub = 0; TreeOctNode* treeNode = node; TreeOctNode* neighbor; - if (node->depth ()>= maxDepth || node->children){return;} + if (node->depth () >= maxDepth || node->children) + { + return; + } // Check if full-depth extraction is enabled and we have an iso-node that is not at maximum depth - if (!sub && fullDepthIso && MarchingCubes::HasRoots (node->nodeData.mcIndex)){sub = 1;} + if (!sub && fullDepthIso && MarchingCubes::HasRoots (node->nodeData.mcIndex)) + { + sub = 1; + } // Check if the node has faces that are ambiguous and are adjacent to finer neighbors - for (i = 0;ifaceNeighbor (i); - if (neighbor && neighbor->children){if (MarchingCubes::IsAmbiguous (node->nodeData.mcIndex,i)){sub = 1;}} + if (neighbor && neighbor->children) + { + if (MarchingCubes::IsAmbiguous (node->nodeData.mcIndex, i)) + { + sub = 1; + } + } } // Check if the node has edges with more than one root - for (i = 0;i1){sub = 1;}} + for (i = 0; i < Cube::EDGES && !sub; i++) + { + if (EdgeRootCount (node, i, maxDepth) > 1) + { + sub = 1; + } + } - for (i = 0;ifaceNeighbor (i); - if ( neighbor && neighbor->children && - !MarchingCubes::HasFaceRoots (node->nodeData.mcIndex,i) && - InteriorFaceRootCount (neighbor,Cube::FaceReflectFaceIndex (i,i),maxDepth)){sub = 1;} + if (neighbor && neighbor->children && + !MarchingCubes::HasFaceRoots (node->nodeData.mcIndex, i) && + InteriorFaceRootCount (neighbor, Cube::FaceReflectFaceIndex (i, i), maxDepth)) + { + sub = 1; + } } - if (sub){ - Subdivide (node,isoValue,maxDepth); - for (i = 0;ifaceNeighbor (i); - if (neighbor && !neighbor->children){Validate (neighbor,isoValue,maxDepth,fullDepthIso);} + if (neighbor && !neighbor->children) + { + Validate (neighbor, isoValue, maxDepth, fullDepthIso); + } } - for (i = 0;iedgeNeighbor (i); - if (neighbor && !neighbor->children){Validate (neighbor,isoValue,maxDepth,fullDepthIso);} + if (neighbor && !neighbor->children) + { + Validate (neighbor, isoValue, maxDepth, fullDepthIso); + } + } + for (i = 0; i < Cube::CORNERS; i++) + { + if (!node->children[i].children) + { + Validate (&node->children[i], isoValue, maxDepth, fullDepthIso); + } } - for (i = 0;ichildren[i].children){Validate (&node->children[i],isoValue,maxDepth,fullDepthIso);}} } } + template - void Octree::Validate (TreeOctNode* node,const Real& isoValue,const int& maxDepth,const int& fullDepthIso,const int& subdivideDepth){ - int i,sub = 0; + void Octree + ::Validate (TreeOctNode* node, const Real& isoValue, const int& maxDepth, const int& fullDepthIso, const int& subdivideDepth) + { + int i, sub = 0; TreeOctNode* treeNode = node; TreeOctNode* neighbor; - if (node->depth ()>= maxDepth || node->children){return;} + if (node->depth () >= maxDepth || node->children) + { + return; + } // Check if full-depth extraction is enabled and we have an iso-node that is not at maximum depth - if (!sub && fullDepthIso && MarchingCubes::HasRoots (node->nodeData.mcIndex)){sub = 1;} + if (!sub && fullDepthIso && MarchingCubes::HasRoots (node->nodeData.mcIndex)) + { + sub = 1; + } // Check if the node has faces that are ambiguous and are adjacent to finer neighbors - for (i = 0;ifaceNeighbor (i); - if (neighbor && neighbor->children){if (MarchingCubes::IsAmbiguous (node->nodeData.mcIndex,i) || IsBoundaryFace (node,i,subdivideDepth)){sub = 1;}} + if (neighbor && neighbor->children) + { + if (MarchingCubes::IsAmbiguous (node->nodeData.mcIndex, i) || IsBoundaryFace (node, i, subdivideDepth)) + { + sub = 1; + } + } } // Check if the node has edges with more than one root - for (i = 0;i1){sub = 1;}} + for (i = 0; i < Cube::EDGES && !sub; i++) + { + if (EdgeRootCount (node, i, maxDepth) > 1) + { + sub = 1; + } + } - for (i = 0;ifaceNeighbor (i); - if ( neighbor && neighbor->children && !MarchingCubes::HasFaceRoots (node->nodeData.mcIndex,i) && - InteriorFaceRootCount (neighbor,Cube::FaceReflectFaceIndex (i,i),maxDepth)){sub = 1;} + if (neighbor && neighbor->children && !MarchingCubes::HasFaceRoots (node->nodeData.mcIndex, i) && + InteriorFaceRootCount (neighbor, Cube::FaceReflectFaceIndex (i, i), maxDepth)) + { + sub = 1; + } } - if (sub){ - Subdivide (node,isoValue,maxDepth); - for (i = 0;ifaceNeighbor (i); - if (neighbor && !neighbor->children){Validate (neighbor,isoValue,maxDepth,fullDepthIso,subdivideDepth);} + if (neighbor && !neighbor->children) + { + Validate (neighbor, isoValue, maxDepth, fullDepthIso, subdivideDepth); + } } - for (i = 0;iedgeNeighbor (i); - if (neighbor && !neighbor->children){Validate (neighbor,isoValue,maxDepth,fullDepthIso,subdivideDepth);} + if (neighbor && !neighbor->children) + { + Validate (neighbor, isoValue, maxDepth, fullDepthIso, subdivideDepth); + } + } + for (i = 0; i < Cube::CORNERS; i++) + { + if (!node->children[i].children) + { + Validate (&node->children[i], isoValue, maxDepth, fullDepthIso, subdivideDepth); + } } - for (i = 0;ichildren[i].children){Validate (&node->children[i],isoValue,maxDepth,fullDepthIso,subdivideDepth);}} } } ////////////////////////////////////////////////////////////////////////////////////// // The assumption made when calling this code is that the edge has at most one root // ////////////////////////////////////////////////////////////////////////////////////// + template - int Octree::GetRoot (const RootInfo& ri,const Real& isoValue,Point3D & position,hash_map > >& normalHash,const int& nonLinearFit){ - int c1,c2; - Cube::EdgeCorners (ri.edgeIndex,c1,c2); - if (!MarchingCubes::HasEdgeRoots (ri.node->nodeData.mcIndex,ri.edgeIndex)){return 0;} + int Octree + ::GetRoot (const RootInfo& ri, const Real& isoValue, Point3D & position, hash_map > >& normalHash, const int& nonLinearFit) + { + int c1, c2; + Cube::EdgeCorners (ri.edgeIndex, c1, c2); + if (!MarchingCubes::HasEdgeRoots (ri.node->nodeData.mcIndex, ri.edgeIndex)) + { + return 0; + } long long key; Point3D n[2]; @@ -2148,136 +2515,192 @@ namespace pcl { cnf.dValueTables = fData.dValueTables; cnf.res2 = fData.res2; - int i,o,i1,i2,rCount = 0; - Polynomial<2> P; + int i, o, i1, i2, rCount = 0; + Polynomial < 2 > P; std::vector roots; - double x0,x1; - Real center,width; + double x0, x1; + Real center, width; Real averageRoot = 0; - Cube::FactorEdgeIndex (ri.edgeIndex,o,i1,i2); + Cube::FactorEdgeIndex (ri.edgeIndex, o, i1, i2); int idx[3]; - key = VertexData::CornerIndex (ri.node,c1,fData.depth,idx); - cnf.index[0] = idx[0]*fData.res; - cnf.index[1] = idx[1]*fData.res; - cnf.index[2] = idx[2]*fData.res; + key = VertexData::CornerIndex (ri.node, c1, fData.depth, idx); + cnf.index[0] = idx[0] * fData.res; + cnf.index[1] = idx[1] * fData.res; + cnf.index[2] = idx[2] * fData.res; - if (normalHash.find (key) == normalHash.end ()){ + if (normalHash.find (key) == normalHash.end ()) + { cnf.value = 0; cnf.normal.coords[0] = cnf.normal.coords[1] = cnf.normal.coords[2] = 0; // Careful here as the normal isn't quite accurate... (i.e. postNormalSmooth is ignored) #if 0 - if (this->width<= 3){getCornerValueAndNormal (ri.node,c1,cnf.value,cnf.normal);} - else{TreeOctNode::ProcessPointAdjacentNodes (fData.depth,idx,&tree,this->width,&cnf);} + if (this->width <= 3) + { + getCornerValueAndNormal (ri.node, c1, cnf.value, cnf.normal); + } + else + { + TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, this->width, &cnf); + } #else - TreeOctNode::ProcessPointAdjacentNodes (fData.depth,idx,&tree,this->width,&cnf); + TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, this->width, &cnf); #endif - normalHash[key] = std::pair > (cnf.value,cnf.normal); + normalHash[key] = std::pair > (cnf.value, cnf.normal); } x0 = normalHash[key].first; n[0] = normalHash[key].second; - key = VertexData::CornerIndex (ri.node,c2,fData.depth,idx); - cnf.index[0] = idx[0]*fData.res; - cnf.index[1] = idx[1]*fData.res; - cnf.index[2] = idx[2]*fData.res; - if (normalHash.find (key) == normalHash.end ()){ + key = VertexData::CornerIndex (ri.node, c2, fData.depth, idx); + cnf.index[0] = idx[0] * fData.res; + cnf.index[1] = idx[1] * fData.res; + cnf.index[2] = idx[2] * fData.res; + if (normalHash.find (key) == normalHash.end ()) + { cnf.value = 0; cnf.normal.coords[0] = cnf.normal.coords[1] = cnf.normal.coords[2] = 0; #if 0 - if (this->width<= 3){getCornerValueAndNormal (ri.node,c2,cnf.value,cnf.normal);} - else{TreeOctNode::ProcessPointAdjacentNodes (fData.depth,idx,&tree,this->width,&cnf);} + if (this->width <= 3) + { + getCornerValueAndNormal (ri.node, c2, cnf.value, cnf.normal); + } + else + { + TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, this->width, &cnf); + } #else - TreeOctNode::ProcessPointAdjacentNodes (fData.depth,idx,&tree,this->width,&cnf); + TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, this->width, &cnf); #endif - normalHash[key] = std::pair > (cnf.value,cnf.normal); + normalHash[key] = std::pair > (cnf.value, cnf.normal); } x1 = normalHash[key].first; n[1] = normalHash[key].second; Point3D c; - ri.node->centerAndWidth (c,width); + ri.node->centerAndWidth (c, width); center = c.coords[o]; - for (i = 0;i= 0 && roots[i]<= 1){ - averageRoot+= Real (roots[i]); + P.getSolutions (isoValue, roots, EPSILON); + for (i = 0; i= 0 && roots[i] <= 1) + { + averageRoot += Real (roots[i]); rCount++; } } - if (rCount && nonLinearFit) {averageRoot/= rCount;} - else {averageRoot = Real ( (x0-isoValue)/ (x0-x1));} + if (rCount && nonLinearFit) + { + averageRoot /= rCount; + } + else + { + averageRoot = Real ((x0 - isoValue) / (x0 - x1)); + } - position.coords[o] = Real (center-width/2+width*averageRoot); + position.coords[o] = Real (center - width / 2 + width * averageRoot); return 1; } template - int Octree::GetRoot (const RootInfo& ri,const Real& isoValue,const int& maxDepth,Point3D& position,hash_map > >& normals, - Point3D* normal,const int& nonLinearFit){ - if (!MarchingCubes::HasRoots (ri.node->nodeData.mcIndex)){return 0;} - return GetRoot (ri,isoValue,position,normals,nonLinearFit); + int Octree + ::GetRoot (const RootInfo& ri, const Real& isoValue, const int& /*maxDepth*/, Point3D& position, hash_map > >& normals, + Point3D* /*normal*/, const int& nonLinearFit) + { + if (!MarchingCubes::HasRoots (ri.node->nodeData.mcIndex)) + { + return 0; + } + return GetRoot (ri, isoValue, position, normals, nonLinearFit); } + template - int Octree::GetRootIndex (const TreeOctNode* node,const int& edgeIndex,const int& maxDepth,const int& sDepth,RootInfo& ri){ - int c1,c2,f1,f2; - const TreeOctNode *temp,*finest; + int Octree + ::GetRootIndex (const TreeOctNode* node, const int& edgeIndex, const int& maxDepth, const int& sDepth, RootInfo& ri) + { + int c1, c2, f1, f2; + const TreeOctNode *temp, *finest; int finestIndex; - Cube::FacesAdjacentToEdge (edgeIndex,f1,f2); + Cube::FacesAdjacentToEdge (edgeIndex, f1, f2); finest = node; finestIndex = edgeIndex; - if (node->depth ()faceNeighbor (f1);} - if (temp && temp->children){ + if (node->depth () < maxDepth) + { + if (IsBoundaryFace (node, f1, sDepth)) + { + temp = NULL; + } + else + { + temp = node->faceNeighbor (f1); + } + if (temp && temp->children) + { finest = temp; - finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex,f1); + finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f1); } - else{ - if (IsBoundaryFace (node,f2,sDepth)){temp = NULL;} - else{temp = node->faceNeighbor (f2);} - if (temp && temp->children){ + else + { + if (IsBoundaryFace (node, f2, sDepth)) + { + temp = NULL; + } + else + { + temp = node->faceNeighbor (f2); + } + if (temp && temp->children) + { finest = temp; - finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex,f2); + finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f2); } - else{ - if (IsBoundaryEdge (node,edgeIndex,sDepth)){temp = NULL;} - else{temp = node->edgeNeighbor (edgeIndex);} - if (temp && temp->children){ + else + { + if (IsBoundaryEdge (node, edgeIndex, sDepth)) + { + temp = NULL; + } + else + { + temp = node->edgeNeighbor (edgeIndex); + } + if (temp && temp->children) + { finest = temp; finestIndex = Cube::EdgeReflectEdgeIndex (edgeIndex); } @@ -2285,70 +2708,97 @@ namespace pcl { } } - Cube::EdgeCorners (finestIndex,c1,c2); - if (finest->children){ - if (GetRootIndex (&finest->children[c1],finestIndex,maxDepth,sDepth,ri)) {return 1;} - else if (GetRootIndex (&finest->children[c2],finestIndex,maxDepth,sDepth,ri)) {return 1;} - else {return 0;} + Cube::EdgeCorners (finestIndex, c1, c2); + if (finest->children) + { + if (GetRootIndex (&finest->children[c1], finestIndex, maxDepth, sDepth, ri)) + { + return 1; + } + else if (GetRootIndex (&finest->children[c2], finestIndex, maxDepth, sDepth, ri)) + { + return 1; + } + else + { + return 0; + } } - else{ - if (! (MarchingCubes::edgeMask[finest->nodeData.mcIndex] & (1<nodeData.mcIndex] & (1 << finestIndex))) + { + return 0; + } - int o,i1,i2; - Cube::FactorEdgeIndex (finestIndex,o,i1,i2); - int d,off[3]; - finest->depthAndOffset (d,off); + int o, i1, i2; + Cube::FactorEdgeIndex (finestIndex, o, i1, i2); + int d, off[3]; + finest->depthAndOffset (d, off); ri.node = finest; ri.edgeIndex = finestIndex; - int eIndex[2],offset; - offset = BinaryNode::Index (d,off[o]); - switch (o){ + int eIndex[2], offset; + offset = BinaryNode::Index (d, off[o]); + switch (o) + { case 0: - eIndex[0] = BinaryNode::CornerIndex (maxDepth+1,d,off[1],i1); - eIndex[1] = BinaryNode::CornerIndex (maxDepth+1,d,off[2],i2); + eIndex[0] = BinaryNode::CornerIndex (maxDepth + 1, d, off[1], i1); + eIndex[1] = BinaryNode::CornerIndex (maxDepth + 1, d, off[2], i2); break; case 1: - eIndex[0] = BinaryNode::CornerIndex (maxDepth+1,d,off[0],i1); - eIndex[1] = BinaryNode::CornerIndex (maxDepth+1,d,off[2],i2); + eIndex[0] = BinaryNode::CornerIndex (maxDepth + 1, d, off[0], i1); + eIndex[1] = BinaryNode::CornerIndex (maxDepth + 1, d, off[2], i2); break; case 2: - eIndex[0] = BinaryNode::CornerIndex (maxDepth+1,d,off[0],i1); - eIndex[1] = BinaryNode::CornerIndex (maxDepth+1,d,off[1],i2); + eIndex[0] = BinaryNode::CornerIndex (maxDepth + 1, d, off[0], i1); + eIndex[1] = BinaryNode::CornerIndex (maxDepth + 1, d, off[1], i2); break; } - ri.key = (long long) (o) | (long long) (eIndex[0])<<5 | (long long) (eIndex[1])<<25 | (long long) (offset)<<45; + ri.key = (long long) (o) | (long long) (eIndex[0]) << 5 | (long long) (eIndex[1]) << 25 | (long long) (offset) << 45; return 1; } } + template - int Octree::GetRootIndex (const TreeOctNode* node,const int& edgeIndex,const int& maxDepth,RootInfo& ri){ - int c1,c2,f1,f2; - const TreeOctNode *temp,*finest; + int Octree + ::GetRootIndex (const TreeOctNode* node, const int& edgeIndex, const int& maxDepth, RootInfo& ri) + { + int c1, c2, f1, f2; + const TreeOctNode *temp, *finest; int finestIndex; // The assumption is that the super-edge has a root along it. - if (! (MarchingCubes::edgeMask[node->nodeData.mcIndex] & (1<nodeData.mcIndex] & (1 << edgeIndex))) + { + return 0; + } - Cube::FacesAdjacentToEdge (edgeIndex,f1,f2); + Cube::FacesAdjacentToEdge (edgeIndex, f1, f2); finest = node; finestIndex = edgeIndex; - if (node->depth ()depth () < maxDepth) + { temp = node->faceNeighbor (f1); - if (temp && temp->children){ + if (temp && temp->children) + { finest = temp; - finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex,f1); + finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f1); } - else{ + else + { temp = node->faceNeighbor (f2); - if (temp && temp->children){ + if (temp && temp->children) + { finest = temp; - finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex,f2); + finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f2); } - else{ + else + { temp = node->edgeNeighbor (edgeIndex); - if (temp && temp->children){ + if (temp && temp->children) + { finest = temp; finestIndex = Cube::EdgeReflectEdgeIndex (edgeIndex); } @@ -2356,67 +2806,101 @@ namespace pcl { } } - Cube::EdgeCorners (finestIndex,c1,c2); - if (finest->children){ - if (GetRootIndex (&finest->children[c1],finestIndex,maxDepth,ri)) {return 1;} - else if (GetRootIndex (&finest->children[c2],finestIndex,maxDepth,ri)) {return 1;} - else {return 0;} + Cube::EdgeCorners (finestIndex, c1, c2); + if (finest->children) + { + if (GetRootIndex (&finest->children[c1], finestIndex, maxDepth, ri)) + { + return 1; + } + else if (GetRootIndex (&finest->children[c2], finestIndex, maxDepth, ri)) + { + return 1; + } + else + { + return 0; + } } - else{ - int o,i1,i2; - Cube::FactorEdgeIndex (finestIndex,o,i1,i2); - int d,off[3]; - finest->depthAndOffset (d,off); + else + { + int o, i1, i2; + Cube::FactorEdgeIndex (finestIndex, o, i1, i2); + int d, off[3]; + finest->depthAndOffset (d, off); ri.node = finest; ri.edgeIndex = finestIndex; - int offset,eIndex[2]; - offset = BinaryNode::Index (d,off[o]); - switch (o){ + int offset, eIndex[2]; + offset = BinaryNode::Index (d, off[o]); + //initialize to remove warnings + eIndex[0] = eIndex[1] = 0; + switch (o) + { case 0: - eIndex[0] = BinaryNode::CornerIndex (maxDepth+1,d,off[1],i1); - eIndex[1] = BinaryNode::CornerIndex (maxDepth+1,d,off[2],i2); + eIndex[0] = BinaryNode::CornerIndex (maxDepth + 1, d, off[1], i1); + eIndex[1] = BinaryNode::CornerIndex (maxDepth + 1, d, off[2], i2); break; case 1: - eIndex[0] = BinaryNode::CornerIndex (maxDepth+1,d,off[0],i1); - eIndex[1] = BinaryNode::CornerIndex (maxDepth+1,d,off[2],i2); + eIndex[0] = BinaryNode::CornerIndex (maxDepth + 1, d, off[0], i1); + eIndex[1] = BinaryNode::CornerIndex (maxDepth + 1, d, off[2], i2); break; case 2: - eIndex[0] = BinaryNode::CornerIndex (maxDepth+1,d,off[0],i1); - eIndex[1] = BinaryNode::CornerIndex (maxDepth+1,d,off[1],i2); + eIndex[0] = BinaryNode::CornerIndex (maxDepth + 1, d, off[0], i1); + eIndex[1] = BinaryNode::CornerIndex (maxDepth + 1, d, off[1], i2); break; } - ri.key = (long long) (o) | (long long) (eIndex[0])<<5 | (long long) (eIndex[1])<<25 | (long long) (offset)<<45; + ri.key = (long long) (o) | (long long) (eIndex[0]) << 5 | (long long) (eIndex[1]) << 25 | (long long) (offset) << 45; return 1; } } + template - int Octree::GetRootPair (const RootInfo& ri,const int& maxDepth,RootInfo& pair){ + int Octree + ::GetRootPair (const RootInfo& ri, const int& maxDepth, RootInfo& pair) + { const TreeOctNode* node = ri.node; - int c1,c2,c; - Cube::EdgeCorners (ri.edgeIndex,c1,c2); - while (node->parent){ - c = int (node-node->parent->children); - if (c!= c1 && c!= c2){return 0;} - if (!MarchingCubes::HasEdgeRoots (node->parent->nodeData.mcIndex,ri.edgeIndex)){ - if (c == c1){return GetRootIndex (&node->parent->children[c2],ri.edgeIndex,maxDepth,pair);} - else{return GetRootIndex (&node->parent->children[c1],ri.edgeIndex,maxDepth,pair);} + int c1, c2, c; + Cube::EdgeCorners (ri.edgeIndex, c1, c2); + while (node->parent) + { + c = int (node - node->parent->children); + if (c != c1 && c != c2) + { + return 0; + } + if (!MarchingCubes::HasEdgeRoots (node->parent->nodeData.mcIndex, ri.edgeIndex)) + { + if (c == c1) + { + return GetRootIndex (&node->parent->children[c2], ri.edgeIndex, maxDepth, pair); + } + else + { + return GetRootIndex (&node->parent->children[c1], ri.edgeIndex, maxDepth, pair); + } } node = node->parent; } return 0; } + template - int Octree::GetRootIndex (const long long& key,hash_map& boundaryRoots,hash_map* interiorRoots,CoredPointIndex& index){ - hash_map::iterator rootIter = boundaryRoots.find (key); - if (rootIter!= boundaryRoots.end ()){ + int Octree + ::GetRootIndex (const long long& key, hash_map& boundaryRoots, hash_map* interiorRoots, CoredPointIndex& index) + { + hash_map::iterator rootIter = boundaryRoots.find (key); + if (rootIter != boundaryRoots.end ()) + { index.inCore = 1; index.index = rootIter->second; return 1; } - else if (interiorRoots){ + else if (interiorRoots) + { rootIter = interiorRoots->find (key); - if (rootIter!= interiorRoots->end ()){ + if (rootIter != interiorRoots->end ()) + { index.inCore = 0; index.index = rootIter->second; return 1; @@ -2424,36 +2908,50 @@ namespace pcl { } return 0; } + template - int Octree::SetMCRootPositions (TreeOctNode* node,const int& sDepth,const Real& isoValue, - hash_map& boundaryRoots,hash_map* interiorRoots, - hash_map > >& boundaryNormalHash,hash_map > >* interiorNormalHash, - std::vector >* interiorPositions, - CoredMeshData* mesh,const int& nonLinearFit){ + int Octree + ::SetMCRootPositions (TreeOctNode* node, const int& sDepth, const Real& isoValue, + hash_map& boundaryRoots, hash_map* interiorRoots, + hash_map > >& boundaryNormalHash, hash_map > >* interiorNormalHash, + std::vector >* interiorPositions, + CoredMeshData* mesh, const int& nonLinearFit) + { Point3D position; - int i,j,k,eIndex; + int i, j, k, eIndex; RootInfo ri; int count = 0; - if (!MarchingCubes::HasRoots (node->nodeData.mcIndex)){return 0;} - for (i = 0;inodeData.mcIndex)) + { + return 0; + } + for (i = 0; i < DIMENSION; i++) + { + for (j = 0; j < 2; j++) + { + for (k = 0; k < 2; k++) + { long long key; - eIndex = Cube::EdgeIndex (i,j,k); - if (GetRootIndex (node,eIndex,fData.depth,ri)){ + eIndex = Cube::EdgeIndex (i, j, k); + if (GetRootIndex (node, eIndex, fData.depth, ri)) + { key = ri.key; - if (!interiorRoots || IsBoundaryEdge (node,i,j,k,sDepth)){ - if (boundaryRoots.find (key) == boundaryRoots.end ()){ - GetRoot (ri,isoValue,fData.depth,position,boundaryNormalHash,NULL,nonLinearFit); + if (!interiorRoots || IsBoundaryEdge (node, i, j, k, sDepth)) + { + if (boundaryRoots.find (key) == boundaryRoots.end ()) + { + GetRoot (ri, isoValue, fData.depth, position, boundaryNormalHash, NULL, nonLinearFit); mesh->inCorePoints.push_back (position); - boundaryRoots[key] = int (mesh->inCorePoints.size ())-1; + boundaryRoots[key] = int (mesh->inCorePoints.size ()) - 1; count++; } } - else{ - if (interiorRoots->find (key) == interiorRoots->end ()){ - GetRoot (ri,isoValue,fData.depth,position,*interiorNormalHash,NULL,nonLinearFit); - (*interiorRoots)[key] = mesh->addOutOfCorePoint (position); + else + { + if (interiorRoots->find (key) == interiorRoots->end ()) + { + GetRoot (ri, isoValue, fData.depth, position, *interiorNormalHash, NULL, nonLinearFit); + (*interiorRoots)[key] = mesh->addOutOfCorePoint (position); interiorPositions->push_back (position); count++; } @@ -2464,33 +2962,44 @@ namespace pcl { } return count; } + template - int Octree::SetBoundaryMCRootPositions (const int& sDepth,const Real& isoValue, - hash_map& boundaryRoots,hash_map > >& boundaryNormalHash, - CoredMeshData* mesh,const int& nonLinearFit){ + int Octree + ::SetBoundaryMCRootPositions (const int& sDepth, const Real& isoValue, + hash_map& boundaryRoots, hash_map > >& boundaryNormalHash, + CoredMeshData* mesh, const int& nonLinearFit) + { Point3D position; - int i,j,k,eIndex,hits = 0; + int i, j, k, eIndex, hits = 0; RootInfo ri; int count = 0; TreeOctNode* node; node = tree.nextLeaf (); - while (node){ - if (MarchingCubes::HasRoots (node->nodeData.mcIndex)){ + while (node) + { + if (MarchingCubes::HasRoots (node->nodeData.mcIndex)) + { hits = 0; - for (i = 0;iinCorePoints.push_back (position); - boundaryRoots[key] = int (mesh->inCorePoints.size ())-1; + boundaryRoots[key] = int (mesh->inCorePoints.size ()) - 1; count++; } } @@ -2499,340 +3008,502 @@ namespace pcl { } } } - if (hits){node = tree.nextLeaf (node);} - else{node = tree.nextBranch (node);} + if (hits) + { + node = tree.nextLeaf (node); + } + else + { + node = tree.nextBranch (node); + } } return count; } + template - void Octree::GetMCIsoEdges (TreeOctNode* node,hash_map& boundaryRoots,hash_map* interiorRoots,const int& sDepth, - std::vector >& edges){ + void Octree + ::GetMCIsoEdges (TreeOctNode* node, hash_map& /*boundaryRoots*/, hash_map* /*interiorRoots*/, const int& sDepth, + std::vector >& edges) + { TreeOctNode* temp; - int count = 0,tris = 0; - int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES]; + int count = 0; //,tris = 0; + int isoTri[DIMENSION * MarchingCubes::MAX_TRIANGLES]; FaceEdgesFunction fef; - int ref,fIndex; - hash_map >::iterator iter; - hash_map > vertexCount; + int ref, fIndex; + hash_map >::iterator iter; + hash_map > vertexCount; fef.edges = &edges; fef.maxDepth = fData.depth; fef.vertexCount = &vertexCount; - count = MarchingCubes::AddTriangleIndices (node->nodeData.mcIndex,isoTri); - for (fIndex = 0;fIndexnodeData.mcIndex, isoTri); + for (fIndex = 0; fIndex < Cube::NEIGHBORS; fIndex++) + { + ref = Cube::FaceReflectFaceIndex (fIndex, fIndex); fef.fIndex = ref; temp = node->faceNeighbor (fIndex); // If the face neighbor exists and has higher resolution than the current node, // get the iso-curve from the neighbor - if (temp && temp->children && !IsBoundaryFace (node,fIndex,sDepth)){temp->processNodeFaces (temp,&fef,ref);} - // Otherwise, get it from the node - else{ - RootInfo ri1,ri2; - for (int j = 0;j (ri1.key,ri2.key)); + if (temp && temp->children && !IsBoundaryFace (node, fIndex, sDepth)) + { + temp->processNodeFaces (temp, &fef, ref); + } + // Otherwise, get it from the node + else + { + RootInfo ri1, ri2; + for (int j = 0; j < count; j++) + { + for (int k = 0; k < 3; k++) + { + if (fIndex == Cube::FaceAdjacentToEdges (isoTri[j * 3 + k], isoTri[j * 3 + ((k + 1) % 3)])) + { + if (GetRootIndex (node, isoTri[j * 3 + k], fData.depth, ri1) && GetRootIndex (node, isoTri[j * 3 + ((k + 1) % 3)], fData.depth, ri2)) + { + edges.push_back (std::pair (ri1.key, ri2.key)); iter = vertexCount.find (ri1.key); - if (iter == vertexCount.end ()){ + if (iter == vertexCount.end ()) + { vertexCount[ri1.key].first = ri1; vertexCount[ri1.key].second = 0; } iter = vertexCount.find (ri2.key); - if (iter == vertexCount.end ()){ + if (iter == vertexCount.end ()) + { vertexCount[ri2.key].first = ri2; vertexCount[ri2.key].second = 0; } vertexCount[ri1.key].second++; vertexCount[ri2.key].second--; } - else{fprintf (stderr,"Bad Edge 1: %d %d\n",ri1.key,ri2.key);} + else + { + fprintf (stderr, "Bad Edge 1: %d %d\n", (int) ri1.key, (int) ri2.key); + } } } } } } - for (int i = 0;i (ri.key,edges[i].first)); + if (iter == vertexCount.end ()) + { + printf ("Vertex pair not in list\n"); + } + else + { + edges.push_back (std::pair (ri.key, edges[i].first)); vertexCount[ri.key].second++; vertexCount[edges[i].first].second--; } } iter = vertexCount.find (edges[i].second); - if (iter == vertexCount.end ()){printf ("Could not find vertex: %lld\n",edges[i].second);} - else if (vertexCount[edges[i].second].second){ + if (iter == vertexCount.end ()) + { + printf ("Could not find vertex: %lld\n", edges[i].second); + } + else if (vertexCount[edges[i].second].second) + { RootInfo ri; - GetRootPair (vertexCount[edges[i].second].first,fData.depth,ri); + GetRootPair (vertexCount[edges[i].second].first, fData.depth, ri); iter = vertexCount.find (ri.key); - if (iter == vertexCount.end ()){printf ("Vertex pair not in list\n");} - else{ - edges.push_back (std::pair (edges[i].second,ri.key)); + if (iter == vertexCount.end ()) + { + printf ("Vertex pair not in list\n"); + } + else + { + edges.push_back (std::pair (edges[i].second, ri.key)); vertexCount[edges[i].second].second++; vertexCount[ri.key].second--; } } } } + template - int Octree::GetMCIsoTriangles (TreeOctNode* node,CoredMeshData* mesh,hash_map& boundaryRoots, - hash_map* interiorRoots,std::vector >* interiorPositions,const int& offSet,const int& sDepth , bool addBarycenter , bool polygonMesh ) - { + int Octree + ::GetMCIsoTriangles (TreeOctNode* node, CoredMeshData* mesh, hash_map& boundaryRoots, + hash_map* interiorRoots, std::vector >* interiorPositions, const int& offSet, const int& sDepth, bool addBarycenter, bool polygonMesh) + { int tris = 0; - std::vector > edges; - std::vector > > edgeLoops; - GetMCIsoEdges (node,boundaryRoots,interiorRoots,sDepth,edges); + std::vector > edges; + std::vector > > edgeLoops; + GetMCIsoEdges (node, boundaryRoots, interiorRoots, sDepth, edges); - GetEdgeLoops (edges,edgeLoops); - for (int i = 0;i edgeIndices; - for (int j = 0;j - int Octree::GetEdgeLoops (std::vector >& edges,std::vector > >& loops){ + int Octree + ::GetEdgeLoops (std::vector >& edges, std::vector > >& loops) + { int loopSize = 0; - long long frontIdx,backIdx; - std::pair e,temp; + long long frontIdx, backIdx; + std::pair e, temp; loops.clear (); - while (edges.size ()){ - std::vector > front,back; + while (edges.size ()) + { + std::vector > front, back; e = edges[0]; - loops.resize (loopSize+1); - edges[0] = edges[edges.size ()-1]; + loops.resize (loopSize + 1); + edges[0] = edges[edges.size () - 1]; edges.pop_back (); frontIdx = e.second; backIdx = e.first; - for (int j = int (edges.size ())-1;j>= 0;j--){ - if (edges[j].first == frontIdx || edges[j].second == frontIdx){ - if (edges[j].first == frontIdx) {temp = edges[j];} - else {temp.first = edges[j].second;temp.second = edges[j].first;} + for (int j = int (edges.size ()) - 1; j >= 0; j--) + { + if (edges[j].first == frontIdx || edges[j].second == frontIdx) + { + if (edges[j].first == frontIdx) + { + temp = edges[j]; + } + else + { + temp.first = edges[j].second; + temp.second = edges[j].first; + } frontIdx = temp.second; front.push_back (temp); - edges[j] = edges[edges.size ()-1]; + edges[j] = edges[edges.size () - 1]; edges.pop_back (); j = int (edges.size ()); } - else if (edges[j].first == backIdx || edges[j].second == backIdx){ - if (edges[j].second == backIdx) {temp = edges[j];} - else {temp.first = edges[j].second;temp.second = edges[j].first;} + else if (edges[j].first == backIdx || edges[j].second == backIdx) + { + if (edges[j].second == backIdx) + { + temp = edges[j]; + } + else + { + temp.first = edges[j].second; + temp.second = edges[j].first; + } backIdx = temp.first; back.push_back (temp); - edges[j] = edges[edges.size ()-1]; + edges[j] = edges[edges.size () - 1]; edges.pop_back (); j = int (edges.size ()); } } - for (int j = int (back.size ())-1;j>= 0;j--){loops[loopSize].push_back (back[j]);} + for (int j = int (back.size ()) - 1; j >= 0; j--) + { + loops[loopSize].push_back (back[j]); + } loops[loopSize].push_back (e); - for (int j = 0;j - int Octree::AddTriangles (CoredMeshData* mesh,std::vector edges[3],std::vector >* interiorPositions,const int& offSet){ + int Octree + ::AddTriangles (CoredMeshData* mesh, std::vector edges[3], std::vector >* interiorPositions, const int& offSet) + { std::vector e; - for (int i = 0;i<3;i++){for (size_t j = 0;j - int Octree::AddTriangles ( CoredMeshData* mesh , std::vector& edges , std::vector >* interiorPositions , const int& offSet , bool addBarycenter , bool polygonMesh ) + int Octree + ::AddTriangles (CoredMeshData* mesh, std::vector& edges, std::vector >* interiorPositions, const int& offSet, bool addBarycenter, bool polygonMesh) { - if ( polygonMesh ) + if (polygonMesh) { - std::vector< CoredVertexIndex > vertices ( edges.size () ); - for ( int i = 0 ; i vertices (edges.size ()); + for (size_t i = 0; i < edges.size (); i++) { - vertices[i].idx = edges[i].index; - vertices[i].inCore = edges[i].inCore; + vertices[i].idx = edges[i].index; + vertices[i].inCore = edges[i].inCore; } - mesh->addPolygon ( vertices ); + mesh->addPolygon (vertices); return 1; } - if ( edges.size ()>3 ) + if (edges.size () > 3) { #if 1 - bool isCoplanar = false; + bool isCoplanar = false; - for ( int i = 0 ; i v1 , v2; - if ( edges[i].inCore ) for ( int k = 0 ; k<3 ; k++ ) v1.coords[k] = mesh->inCorePoints[ edges[i].index ].coords[k]; - else for ( int k = 0 ; k<3 ; k++ ) v1.coords[k] = (*interiorPositions)[ edges[i].index-offSet ].coords[k]; - if ( edges[j].inCore ) for ( int k = 0 ; k<3 ; k++ ) v2.coords[k] = mesh->inCorePoints[ edges[j].index ].coords[k]; - else for ( int k = 0 ; k<3 ; k++ ) v2.coords[k] = (*interiorPositions)[ edges[j].index-offSet ].coords[k]; - for ( int k = 0 ; k<3 ; k++ ) if ( v1.coords[k] == v2.coords[k] ) isCoplanar = true; + Point3D< Real > v1, v2; + if (edges[i].inCore) for (int k = 0; k < 3; k++) v1.coords[k] = mesh->inCorePoints[ edges[i].index ].coords[k]; + else for (int k = 0; k < 3; k++) v1.coords[k] = (*interiorPositions)[ edges[i].index - offSet ].coords[k]; + if (edges[j].inCore) for (int k = 0; k < 3; k++) v2.coords[k] = mesh->inCorePoints[ edges[j].index ].coords[k]; + else for (int k = 0; k < 3; k++) v2.coords[k] = (*interiorPositions)[ edges[j].index - offSet ].coords[k]; + for (int k = 0; k < 3; k++) if (v1.coords[k] == v2.coords[k]) isCoplanar = true; } - if ( addBarycenter && isCoplanar ) + if (addBarycenter && isCoplanar) #else - if ( addBarycenter ) + if (addBarycenter) #endif + { + Point3D< Real > c; + c.coords[0] = c.coords[1] = c.coords[2] = 0; + for (int i = 0; i p; + if (edges[i].inCore) for (int j = 0; j < 3; j++) p.coords[j] = mesh->inCorePoints[edges[i].index].coords[j]; + else for (int j = 0; j < 3; j++) p.coords[j] = (*interiorPositions)[edges[i].index - offSet].coords[j]; + c.coords[0] += p.coords[0], c.coords[1] += p.coords[1], c.coords[2] += p.coords[2]; + } + c.coords[0] /= edges.size (), c.coords[1] /= edges.size (), c.coords[2] /= edges.size (); + int cIdx = mesh->addOutOfCorePoint (c); + for (int i = 0; i vertices (3); + vertices[0].idx = edges[i].index; + vertices[1].idx = edges[ (i + 1) % edges.size ()].index; + vertices[2].idx = cIdx; + vertices[0].inCore = edges[i ].inCore; + vertices[1].inCore = edges[ (i + 1) % edges.size ()].inCore; + vertices[2].inCore = 0; + mesh->addPolygon (vertices); + } + return edges.size (); + } + else + { + Triangulation t; + + // Add the points to the triangulation + for (int i = 0; i c; - c.coords[0] = c.coords[1] = c.coords[2] = 0; - for ( int i = 0 ; i p; + if (edges[i].inCore) { - Point3D p; - if (edges[i].inCore) for (int j = 0 ; j<3 ; j++ ) p.coords[j] = mesh->inCorePoints[edges[i].index].coords[j]; - else for (int j = 0 ; j<3 ; j++ ) p.coords[j] = (*interiorPositions)[edges[i].index-offSet].coords[j]; - c.coords[0] += p.coords[0] , c.coords[1] += p.coords[1] , c.coords[2] += p.coords[2]; + for (int j = 0; j < 3; j++) + { + p.coords[j] = mesh->inCorePoints[edges[i].index].coords[j]; + } } - c.coords[0] /= edges.size () , c.coords[1] /= edges.size () , c.coords[2] /= edges.size (); - int cIdx = mesh->addOutOfCorePoint ( c ); - for ( int i = 0 ; i vertices ( 3 ); - vertices[0].idx = edges[i].index; - vertices[1].idx = edges[ (i+1)%edges.size ()].index; - vertices[2].idx = cIdx; - vertices[0].inCore = edges[i ].inCore; - vertices[1].inCore = edges[ (i+1)%edges.size ()].inCore; - vertices[2].inCore = 0; - mesh->addPolygon ( vertices ); + for (int j = 0; j < 3; j++) + { + p.coords[j] = (*interiorPositions)[edges[i].index - offSet].coords[j]; + } } - return edges.size (); + t.points.push_back (p); } - else - { - Triangulation t; - - // Add the points to the triangulation - for (int i = 0;i p; - if (edges[i].inCore) {for (int j = 0;j<3;j++){p.coords[j] = mesh->inCorePoints[edges[i].index].coords[j];}} - else {for (int j = 0;j<3;j++){p.coords[j] = (*interiorPositions)[edges[i].index-offSet].coords[j];}} - t.points.push_back (p); - } - // Create a fan triangulation - for (int i = 1;i vertices ( 3 ); - int idx[3]; - t.factor ( i , idx[0] , idx[1] , idx[2] ); - for ( int j = 0 ; j<3 ; j++ ) + if (t.flipMinimize (i)) { - vertices[j].idx = edges[ idx[j] ].index; - vertices[j].inCore = edges[ idx[j] ].inCore; + break; } - mesh->addPolygon ( vertices ); } + if (i == t.edges.size ()) + { + break; + } + } + // Add the triangles to the mesh + for (int i = 0; i vertices (3); + int idx[3]; + t.factor (i, idx[0], idx[1], idx[2]); + for (int j = 0; j < 3; j++) + { + vertices[j].idx = edges[ idx[j] ].index; + vertices[j].inCore = edges[ idx[j] ].inCore; + } + mesh->addPolygon (vertices); } + } } - else if ( edges.size () == 3 ) + else if (edges.size () == 3) { - std::vector< CoredVertexIndex > vertices ( 3 ); - for ( int i = 0 ; i<3 ; i++ ) + std::vector< CoredVertexIndex > vertices (3); + for (int i = 0; i < 3; i++) { - vertices[i].idx = edges[i].index; - vertices[i].inCore = edges[i].inCore; + vertices[i].idx = edges[i].index; + vertices[i].inCore = edges[i].inCore; } - mesh->addPolygon ( vertices ); + mesh->addPolygon (vertices); } - return int (edges.size ())-2; + return int (edges.size ()) - 2; } //////////////// // VertexData // //////////////// - long long VertexData::CenterIndex (const TreeOctNode* node,const int& maxDepth){ + + long long + VertexData::CenterIndex (const TreeOctNode* node, const int& maxDepth) + { int idx[DIMENSION]; - return CenterIndex (node,maxDepth,idx); + return CenterIndex (node, maxDepth, idx); } - long long VertexData::CenterIndex (const TreeOctNode* node,const int& maxDepth,int idx[DIMENSION]){ - int d,o[3]; - node->depthAndOffset (d,o); - for (int i = 0;i::CornerIndex (maxDepth+1,d+1,o[i]<<1,1);} - return (long long) (idx[0]) | (long long) (idx[1])<<15 | (long long) (idx[2])<<30; + + long long + VertexData::CenterIndex (const TreeOctNode* node, const int& maxDepth, int idx[DIMENSION]) + { + int d, o[3]; + node->depthAndOffset (d, o); + for (int i = 0; i < DIMENSION; i++) + { + idx[i] = BinaryNode::CornerIndex (maxDepth + 1, d + 1, o[i] << 1, 1); + } + return (long long) (idx[0]) | (long long) (idx[1]) << 15 | (long long) (idx[2]) << 30; } - long long VertexData::CenterIndex (const int& depth,const int offSet[DIMENSION],const int& maxDepth,int idx[DIMENSION]){ - for (int i = 0;i::CornerIndex (maxDepth+1,depth+1,offSet[i]<<1,1);} - return (long long) (idx[0]) | (long long) (idx[1])<<15 | (long long) (idx[2])<<30; + + long long + VertexData::CenterIndex (const int& depth, const int offSet[DIMENSION], const int& maxDepth, int idx[DIMENSION]) + { + for (int i = 0; i < DIMENSION; i++) + { + idx[i] = BinaryNode::CornerIndex (maxDepth + 1, depth + 1, offSet[i] << 1, 1); + } + return (long long) (idx[0]) | (long long) (idx[1]) << 15 | (long long) (idx[2]) << 30; } - long long VertexData::CornerIndex (const TreeOctNode* node,const int& cIndex,const int& maxDepth){ + + long long + VertexData::CornerIndex (const TreeOctNode* node, const int& cIndex, const int& maxDepth) + { int idx[DIMENSION]; - return CornerIndex (node,cIndex,maxDepth,idx); + return CornerIndex (node, cIndex, maxDepth, idx); } - long long VertexData::CornerIndex (const TreeOctNode* node,const int& cIndex,const int& maxDepth,int idx[DIMENSION]){ + + long long + VertexData::CornerIndex (const TreeOctNode* node, const int& cIndex, const int& maxDepth, int idx[DIMENSION]) + { int x[DIMENSION]; - Cube::FactorCornerIndex (cIndex,x[0],x[1],x[2]); - int d,o[3]; - node->depthAndOffset (d,o); - for (int i = 0;i::CornerIndex (maxDepth+1,d,o[i],x[i]);} - return (long long) (idx[0]) | (long long) (idx[1])<<15 | (long long) (idx[2])<<30; + Cube::FactorCornerIndex (cIndex, x[0], x[1], x[2]); + int d, o[3]; + node->depthAndOffset (d, o); + for (int i = 0; i < DIMENSION; i++) + { + idx[i] = BinaryNode::CornerIndex (maxDepth + 1, d, o[i], x[i]); + } + return (long long) (idx[0]) | (long long) (idx[1]) << 15 | (long long) (idx[2]) << 30; } - long long VertexData::CornerIndex (const int& depth,const int offSet[DIMENSION],const int& cIndex,const int& maxDepth,int idx[DIMENSION]){ + + long long + VertexData::CornerIndex (const int& depth, const int offSet[DIMENSION], const int& cIndex, const int& maxDepth, int idx[DIMENSION]) + { int x[DIMENSION]; - Cube::FactorCornerIndex (cIndex,x[0],x[1],x[2]); - for (int i = 0;i::CornerIndex (maxDepth+1,depth,offSet[i],x[i]);} - return (long long) (idx[0]) | (long long) (idx[1])<<15 | (long long) (idx[2])<<30; + Cube::FactorCornerIndex (cIndex, x[0], x[1], x[2]); + for (int i = 0; i < DIMENSION; i++) + { + idx[i] = BinaryNode::CornerIndex (maxDepth + 1, depth, offSet[i], x[i]); + } + return (long long) (idx[0]) | (long long) (idx[1]) << 15 | (long long) (idx[2]) << 30; } - long long VertexData::FaceIndex (const TreeOctNode* node,const int& fIndex,const int& maxDepth){ + + long long + VertexData::FaceIndex (const TreeOctNode* node, const int& fIndex, const int& maxDepth) + { int idx[DIMENSION]; - return FaceIndex (node,fIndex,maxDepth,idx); - } - long long VertexData::FaceIndex (const TreeOctNode* node,const int& fIndex,const int& maxDepth,int idx[DIMENSION]){ - int dir,offset; - Cube::FactorFaceIndex (fIndex,dir,offset); - int d,o[3]; - node->depthAndOffset (d,o); - for (int i = 0;i::CornerIndex (maxDepth+1,d+1,o[i]<<1,1);} - idx[dir] = BinaryNode::CornerIndex (maxDepth+1,d,o[dir],offset); - return (long long) (idx[0]) | (long long) (idx[1])<<15 | (long long) (idx[2])<<30; - } - long long VertexData::EdgeIndex (const TreeOctNode* node,const int& eIndex,const int& maxDepth){ + return FaceIndex (node, fIndex, maxDepth, idx); + } + + long long + VertexData::FaceIndex (const TreeOctNode* node, const int& fIndex, const int& maxDepth, int idx[DIMENSION]) + { + int dir, offset; + Cube::FactorFaceIndex (fIndex, dir, offset); + int d, o[3]; + node->depthAndOffset (d, o); + for (int i = 0; i < DIMENSION; i++) + { + idx[i] = BinaryNode::CornerIndex (maxDepth + 1, d + 1, o[i] << 1, 1); + } + idx[dir] = BinaryNode::CornerIndex (maxDepth + 1, d, o[dir], offset); + return (long long) (idx[0]) | (long long) (idx[1]) << 15 | (long long) (idx[2]) << 30; + } + + long long + VertexData::EdgeIndex (const TreeOctNode* node, const int& eIndex, const int& maxDepth) + { int idx[DIMENSION]; - return EdgeIndex (node,eIndex,maxDepth,idx); - } - long long VertexData::EdgeIndex (const TreeOctNode* node,const int& eIndex,const int& maxDepth,int idx[DIMENSION]){ - int o,i1,i2; - int d,off[3]; - node->depthAndOffset (d,off); - for (int i = 0;i::CornerIndex (maxDepth+1,d+1,off[i]<<1,1);} - Cube::FactorEdgeIndex (eIndex,o,i1,i2); - switch (o){ + return EdgeIndex (node, eIndex, maxDepth, idx); + } + + long long + VertexData::EdgeIndex (const TreeOctNode* node, const int& eIndex, const int& maxDepth, int idx[DIMENSION]) + { + int o, i1, i2; + int d, off[3]; + node->depthAndOffset (d, off); + for (int i = 0; i < DIMENSION; i++) + { + idx[i] = BinaryNode::CornerIndex (maxDepth + 1, d + 1, off[i] << 1, 1); + } + Cube::FactorEdgeIndex (eIndex, o, i1, i2); + switch (o) + { case 0: - idx[1] = BinaryNode::CornerIndex (maxDepth+1,d,off[1],i1); - idx[2] = BinaryNode::CornerIndex (maxDepth+1,d,off[2],i2); + idx[1] = BinaryNode::CornerIndex (maxDepth + 1, d, off[1], i1); + idx[2] = BinaryNode::CornerIndex (maxDepth + 1, d, off[2], i2); break; case 1: - idx[0] = BinaryNode::CornerIndex (maxDepth+1,d,off[0],i1); - idx[2] = BinaryNode::CornerIndex (maxDepth+1,d,off[2],i2); + idx[0] = BinaryNode::CornerIndex (maxDepth + 1, d, off[0], i1); + idx[2] = BinaryNode::CornerIndex (maxDepth + 1, d, off[2], i2); break; case 2: - idx[0] = BinaryNode::CornerIndex (maxDepth+1,d,off[0],i1); - idx[1] = BinaryNode::CornerIndex (maxDepth+1,d,off[1],i2); + idx[0] = BinaryNode::CornerIndex (maxDepth + 1, d, off[0], i1); + idx[1] = BinaryNode::CornerIndex (maxDepth + 1, d, off[1], i2); break; }; - return (long long) (idx[0]) | (long long) (idx[1])<<15 | (long long) (idx[2])<<30; + return (long long) (idx[0]) | (long long) (idx[1]) << 15 | (long long) (idx[2]) << 30; } diff --git a/surface/include/pcl/surface/impl/poisson/octree_poisson.hpp b/surface/include/pcl/surface/impl/poisson/octree_poisson.hpp index 26a1bd3625b..2839ba9480b 100644 --- a/surface/include/pcl/surface/impl/poisson/octree_poisson.hpp +++ b/surface/include/pcl/surface/impl/poisson/octree_poisson.hpp @@ -44,1261 +44,2620 @@ #include -namespace pcl { - namespace poisson { +namespace pcl +{ + namespace poisson + { ///////////// // OctNode // ///////////// - template const int OctNode::DepthShift=5; - template const int OctNode::OffsetShift=19; - template const int OctNode::DepthMask=(1< const int OctNode::OffsetMask=(1< const int OctNode::OffsetShift1=DepthShift; - template const int OctNode::OffsetShift2=OffsetShift1+OffsetShift; - template const int OctNode::OffsetShift3=OffsetShift2+OffsetShift; + template const int OctNode::DepthShift = 5; + template const int OctNode::OffsetShift = 19; + template const int OctNode::DepthMask = (1 << DepthShift) - 1; + template const int OctNode::OffsetMask = (1 << OffsetShift) - 1; + template const int OctNode::OffsetShift1 = DepthShift; + template const int OctNode::OffsetShift2 = OffsetShift1 + OffsetShift; + template const int OctNode::OffsetShift3 = OffsetShift2 + OffsetShift; - template int OctNode::UseAlloc=0; - template Allocator > OctNode::AllocatorOctNode; + template int OctNode::UseAlloc = 0; + template Allocator > OctNode::AllocatorOctNode; - template - void OctNode::SetAllocator(int blockSize) + template + void OctNode + ::SetAllocator (int blockSize) { - if(blockSize>0) + if (blockSize > 0) { - UseAlloc=1; - AllocatorOctNode.set(blockSize); + UseAlloc = 1; + AllocatorOctNode.set (blockSize); } - else{UseAlloc=0;} + else + { + UseAlloc = 0; + } + } + + template + int OctNode + ::UseAllocator (void) + { + return UseAlloc; } - template - int OctNode::UseAllocator(void){return UseAlloc;} - template - OctNode::OctNode(void){ - parent=children=NULL; - d=off[0]=off[1]=off[2]=0; + template + OctNode + ::OctNode (void) + { + parent = children = NULL; + d = off[0] = off[1] = off[2] = 0; } - template - OctNode::~OctNode(void){ - if(!UseAlloc){if(children){delete[] children;}} - parent=children=NULL; + template + OctNode + ::~OctNode (void) + { + if (!UseAlloc) + { + if (children) + { + delete[] children; + } + } + parent = children = NULL; } - template - void OctNode::setFullDepth(const int& maxDepth){ - if(maxDepth){ - if(!children){initChildren();} - for(int i=0;i<8;i++){children[i].setFullDepth(maxDepth-1);} + + template + void OctNode + ::setFullDepth (const int& maxDepth) + { + if (maxDepth) + { + if (!children) + { + initChildren (); + } + for (int i = 0; i < 8; i++) + { + children[i].setFullDepth (maxDepth - 1); + } } } - template - int OctNode::initChildren(void){ - int i,j,k; + template + int OctNode + ::initChildren (void) + { + int i, j, k; - if(UseAlloc){children=AllocatorOctNode.newElements(8);} - else{ - if(children){delete[] children;} - children=NULL; - children=new OctNode[Cube::CORNERS]; + if (UseAlloc) + { + children = AllocatorOctNode.newElements (8); + } + else + { + if (children) + { + delete[] children; + } + children = NULL; + children = new OctNode[Cube::CORNERS]; } - if(!children){ - fprintf(stderr,"Failed to initialize children in OctNode::initChildren\n"); - exit(0); + if (!children) + { + fprintf (stderr, "Failed to initialize children in OctNode::initChildren\n"); + exit (0); return 0; } - int d,off[3]; - depthAndOffset(d,off); - for(i=0;i<2;i++){ - for(j=0;j<2;j++){ - for(k=0;k<2;k++){ - int idx=Cube::CornerIndex(i,j,k); - children[idx].parent=this; - children[idx].children=NULL; + int d, off[3]; + depthAndOffset (d, off); + for (i = 0; i < 2; i++) + { + for (j = 0; j < 2; j++) + { + for (k = 0; k < 2; k++) + { + int idx = Cube::CornerIndex (i, j, k); + children[idx].parent = this; + children[idx].children = NULL; int off2[3]; - off2[0]=(off[0]<<1)+i; - off2[1]=(off[1]<<1)+j; - off2[2]=(off[2]<<1)+k; - Index(d+1,off2,children[idx].d,children[idx].off); + off2[0] = (off[0] << 1) + i; + off2[1] = (off[1] << 1) + j; + off2[2] = (off[2] << 1) + k; + Index (d + 1, off2, children[idx].d, children[idx].off); } } } return 1; } - template - inline void OctNode::Index(const int& depth,const int offset[3],short& d,short off[3]){ - d=short(depth); - off[0]=short((1< - inline void OctNode::depthAndOffset(int& depth,int offset[3]) const { - depth=int(d); - offset[0]=(int(off[0])+1)&(~(1< - inline int OctNode::depth(void) const {return int(d);} - template - inline void OctNode::DepthAndOffset(const long long& index,int& depth,int offset[3]){ - depth=int(index&DepthMask); - offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<>OffsetShift2)&OffsetMask)+1)&(~(1<>OffsetShift3)&OffsetMask)+1)&(~(1< - inline int OctNode::Depth(const long long& index){return int(index&DepthMask);} - template - void OctNode::centerAndWidth(Point3D& center,Real& width) const{ - int depth,offset[3]; - depth=int(d); - offset[0]=(int(off[0])+1)&(~(1< - inline void OctNode::CenterAndWidth(const long long& index,Point3D& center,Real& width){ - int depth,offset[3]; - depth=index&DepthMask; - offset[0]=(int((index>>OffsetShift1)&OffsetMask)+1)&(~(1<>OffsetShift2)&OffsetMask)+1)&(~(1<>OffsetShift3)&OffsetMask)+1)&(~(1< - int OctNode::maxDepth(void) const{ - if(!children){return 0;} - else{ - int c,d; - for(int i=0;ic){c=d;} - } - return c+1; - } - } - template - int OctNode::nodes(void) const{ - if(!children){return 1;} - else{ - int c=0; - for(int i=0;i - int OctNode::leaves(void) const{ - if(!children){return 1;} - else{ - int c=0; - for(int i=0;i + inline void OctNode + ::Index (const int& depth, const int offset[3], short& d, short off[3]) + { + d = short(depth); + off[0] = short((1 << depth) + offset[0] - 1); + off[1] = short((1 << depth) + offset[1] - 1); + off[2] = short((1 << depth) + offset[2] - 1); + } + + template + inline void OctNode + ::depthAndOffset (int& depth, int offset[3]) const + { + depth = int(d); + offset[0] = (int(off[0]) + 1)&(~(1 << depth)); + offset[1] = (int(off[1]) + 1)&(~(1 << depth)); + offset[2] = (int(off[2]) + 1)&(~(1 << depth)); + } + + template + inline int OctNode + ::depth (void) const + { + return int(d); + } + + template + inline void OctNode + ::DepthAndOffset (const long long& index, int& depth, int offset[3]) + { + depth = int(index & DepthMask); + offset[0] = (int((index >> OffsetShift1) & OffsetMask) + 1)&(~(1 << depth)); + offset[1] = (int((index >> OffsetShift2) & OffsetMask) + 1)&(~(1 << depth)); + offset[2] = (int((index >> OffsetShift3) & OffsetMask) + 1)&(~(1 << depth)); + } + + template + inline int OctNode + ::Depth (const long long& index) + { + return int(index & DepthMask); + } + + template + void OctNode + ::centerAndWidth (Point3D& center, Real& width) const + { + int depth, offset[3]; + depth = int(d); + offset[0] = (int(off[0]) + 1)&(~(1 << depth)); + offset[1] = (int(off[1]) + 1)&(~(1 << depth)); + offset[2] = (int(off[2]) + 1)&(~(1 << depth)); + width = Real (1.0 / (1 << depth)); + for (int dim = 0; dim < DIMENSION; dim++) + { + center.coords[dim] = Real (0.5 + offset[dim]) * width; + } + } + + template + inline void OctNode + ::CenterAndWidth (const long long& index, Point3D& center, Real& width) + { + int depth, offset[3]; + depth = index&DepthMask; + offset[0] = (int((index >> OffsetShift1) & OffsetMask) + 1)&(~(1 << depth)); + offset[1] = (int((index >> OffsetShift2) & OffsetMask) + 1)&(~(1 << depth)); + offset[2] = (int((index >> OffsetShift3) & OffsetMask) + 1)&(~(1 << depth)); + width = Real (1.0 / (1 << depth)); + for (int dim = 0; dim < DIMENSION; dim++) + { + center.coords[dim] = Real (0.5 + offset[dim]) * width; + } + } + + template + int OctNode + ::maxDepth (void) const + { + if (!children) + { + return 0; + } + else + { + int c = 0, d; + for (int i = 0; i < Cube::CORNERS; i++) + { + d = children[i].maxDepth (); + if (!i || d > c) + { + c = d; + } + } + return c + 1; + } + } + + template + int OctNode + ::nodes (void) const + { + if (!children) + { + return 1; + } + else + { + int c = 0; + for (int i = 0; i < Cube::CORNERS; i++) + { + c += children[i].nodes (); + } + return c + 1; + } + } + + template + int OctNode + ::leaves (void) const + { + if (!children) + { + return 1; + } + else + { + int c = 0; + for (int i = 0; i < Cube::CORNERS; i++) + { + c += children[i].leaves (); + } return c; } } - template - int OctNode::maxDepthLeaves(const int& maxDepth) const{ - if(depth()>maxDepth){return 0;} - if(!children){return 1;} - else{ - int c=0; - for(int i=0;i + int OctNode + ::maxDepthLeaves (const int& maxDepth) const + { + if (depth () > maxDepth) + { + return 0; + } + if (!children) + { + return 1; + } + else + { + int c = 0; + for (int i = 0; i < Cube::CORNERS; i++) + { + c += children[i].maxDepthLeaves (maxDepth); + } return c; } } - template - const OctNode* OctNode::root(void) const{ - const OctNode* temp=this; - while(temp->parent){temp=temp->parent;} + + template + const OctNode* OctNode + ::root (void) const + { + const OctNode* temp = this; + while (temp->parent) + { + temp = temp->parent; + } return temp; } - - template - const OctNode* OctNode::nextBranch(const OctNode* current) const{ - if(!current->parent || current==this){return NULL;} - if(current-current->parent->children==Cube::CORNERS-1){return nextBranch(current->parent);} - else{return current+1;} + template + const OctNode* OctNode + ::nextBranch (const OctNode* current) const + { + if (!current->parent || current == this) + { + return NULL; + } + if (current - current->parent->children == Cube::CORNERS - 1) + { + return nextBranch (current->parent); + } + else + { + return current + 1; + } } - template - OctNode* OctNode::nextBranch(OctNode* current){ - if(!current->parent || current==this){return NULL;} - if(current-current->parent->children==Cube::CORNERS-1){return nextBranch(current->parent);} - else{return current+1;} + + template + OctNode* OctNode + ::nextBranch (OctNode* current) + { + if (!current->parent || current == this) + { + return NULL; + } + if (current - current->parent->children == Cube::CORNERS - 1) + { + return nextBranch (current->parent); + } + else + { + return current + 1; + } } - template - const OctNode* OctNode::nextLeaf(const OctNode* current) const{ - if(!current){ - const OctNode* temp=this; - while(temp->children){temp=&temp->children[0];} + + template + const OctNode* OctNode + ::nextLeaf (const OctNode* current) const + { + if (!current) + { + const OctNode* temp = this; + while (temp->children) + { + temp = &temp->children[0]; + } return temp; } - if(current->children){return current->nextLeaf();} - const OctNode* temp=nextBranch(current); - if(!temp){return NULL;} - else{return temp->nextLeaf();} + if (current->children) + { + return current->nextLeaf (); + } + const OctNode* temp = nextBranch (current); + if (!temp) + { + return NULL; + } + else + { + return temp->nextLeaf (); + } } - template - OctNode* OctNode::nextLeaf(OctNode* current){ - if(!current){ - OctNode* temp=this; - while(temp->children){temp=&temp->children[0];} + + template + OctNode* OctNode + ::nextLeaf (OctNode* current) + { + if (!current) + { + OctNode* temp = this; + while (temp->children) + { + temp = &temp->children[0]; + } return temp; } - if(current->children){return current->nextLeaf();} - OctNode* temp=nextBranch(current); - if(!temp){return NULL;} - else{return temp->nextLeaf();} + if (current->children) + { + return current->nextLeaf (); + } + OctNode* temp = nextBranch (current); + if (!temp) + { + return NULL; + } + else + { + return temp->nextLeaf (); + } } - template - const OctNode* OctNode::nextNode(const OctNode* current) const{ - if(!current){return this;} - else if(current->children){return ¤t->children[0];} - else{return nextBranch(current);} + template + const OctNode* OctNode + ::nextNode (const OctNode* current) const + { + if (!current) + { + return this; + } + else if (current->children) + { + return ¤t->children[0]; + } + else + { + return nextBranch (current); + } } - template - OctNode* OctNode::nextNode(OctNode* current){ - if(!current){return this;} - else if(current->children){return ¤t->children[0];} - else{return nextBranch(current);} + + template + OctNode* OctNode + ::nextNode (OctNode* current) + { + if (!current) + { + return this; + } + else if (current->children) + { + return ¤t->children[0]; + } + else + { + return nextBranch (current); + } } - template - void OctNode::printRange(void) const{ + template + void OctNode + ::printRange (void) const + { Point3D center; Real width; - centerAndWidth(center,width); - for(int dim=0;dim - void OctNode::AdjacencyCountFunction::Function(const OctNode* node1,const OctNode* node2){count++;} + template + void OctNode + ::AdjacencyCountFunction::Function (const OctNode* node1, const OctNode* node2) + { + count++; + } - template + template template - void OctNode::processNodeNodes(OctNode* node,NodeAdjacencyFunction* F,const int& processCurrent){ - if(processCurrent){F->Function(this,node);} - if(children){__processNodeNodes(node,F);} + void OctNode + ::processNodeNodes (OctNode* node, NodeAdjacencyFunction* F, const int& processCurrent) + { + if (processCurrent) + { + F->Function (this, node); + } + if (children) + { + __processNodeNodes (node, F); + } } - template + + template template - void OctNode::processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,const int& fIndex,const int& processCurrent){ - if(processCurrent){F->Function(this,node);} - if(children){ - int c1,c2,c3,c4; - Cube::FaceCorners(fIndex,c1,c2,c3,c4); - __processNodeFaces(node,F,c1,c2,c3,c4); + void OctNode + ::processNodeFaces (OctNode* node, NodeAdjacencyFunction* F, const int& fIndex, const int& processCurrent) + { + if (processCurrent) + { + F->Function (this, node); + } + if (children) + { + int c1, c2, c3, c4; + Cube::FaceCorners (fIndex, c1, c2, c3, c4); + __processNodeFaces (node, F, c1, c2, c3, c4); } } - template + + template template - void OctNode::processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,const int& eIndex,const int& processCurrent){ - if(processCurrent){F->Function(this,node);} - if(children){ - int c1,c2; - Cube::EdgeCorners(eIndex,c1,c2); - __processNodeEdges(node,F,c1,c2); + void OctNode + ::processNodeEdges (OctNode* node, NodeAdjacencyFunction* F, const int& eIndex, const int& processCurrent) + { + if (processCurrent) + { + F->Function (this, node); + } + if (children) + { + int c1, c2; + Cube::EdgeCorners (eIndex, c1, c2); + __processNodeEdges (node, F, c1, c2); } } - template + + template template - void OctNode::processNodeCorners(OctNode* node,NodeAdjacencyFunction* F,const int& cIndex,const int& processCurrent){ - if(processCurrent){F->Function(this,node);} - OctNode* temp=this; - while(temp->children){ - temp=&temp->children[cIndex]; - F->Function(temp,node); + void OctNode + ::processNodeCorners (OctNode* node, NodeAdjacencyFunction* F, const int& cIndex, const int& processCurrent) + { + if (processCurrent) + { + F->Function (this, node); + } + OctNode* temp = this; + while (temp->children) + { + temp = &temp->children[cIndex]; + F->Function (temp, node); } } - template + + template template - void OctNode::__processNodeNodes(OctNode* node,NodeAdjacencyFunction* F){ - F->Function(&children[0],node); - F->Function(&children[1],node); - F->Function(&children[2],node); - F->Function(&children[3],node); - F->Function(&children[4],node); - F->Function(&children[5],node); - F->Function(&children[6],node); - F->Function(&children[7],node); - if(children[0].children){children[0].__processNodeNodes(node,F);} - if(children[1].children){children[1].__processNodeNodes(node,F);} - if(children[2].children){children[2].__processNodeNodes(node,F);} - if(children[3].children){children[3].__processNodeNodes(node,F);} - if(children[4].children){children[4].__processNodeNodes(node,F);} - if(children[5].children){children[5].__processNodeNodes(node,F);} - if(children[6].children){children[6].__processNodeNodes(node,F);} - if(children[7].children){children[7].__processNodeNodes(node,F);} - } - template + void OctNode + ::__processNodeNodes (OctNode* node, NodeAdjacencyFunction* F) + { + F->Function (&children[0], node); + F->Function (&children[1], node); + F->Function (&children[2], node); + F->Function (&children[3], node); + F->Function (&children[4], node); + F->Function (&children[5], node); + F->Function (&children[6], node); + F->Function (&children[7], node); + if (children[0].children) + { + children[0].__processNodeNodes (node, F); + } + if (children[1].children) + { + children[1].__processNodeNodes (node, F); + } + if (children[2].children) + { + children[2].__processNodeNodes (node, F); + } + if (children[3].children) + { + children[3].__processNodeNodes (node, F); + } + if (children[4].children) + { + children[4].__processNodeNodes (node, F); + } + if (children[5].children) + { + children[5].__processNodeNodes (node, F); + } + if (children[6].children) + { + children[6].__processNodeNodes (node, F); + } + if (children[7].children) + { + children[7].__processNodeNodes (node, F); + } + } + + template template - void OctNode::__processNodeEdges(OctNode* node,NodeAdjacencyFunction* F,const int& cIndex1,const int& cIndex2){ - F->Function(&children[cIndex1],node); - F->Function(&children[cIndex2],node); - if(children[cIndex1].children){children[cIndex1].__processNodeEdges(node,F,cIndex1,cIndex2);} - if(children[cIndex2].children){children[cIndex2].__processNodeEdges(node,F,cIndex1,cIndex2);} + void OctNode + ::__processNodeEdges (OctNode* node, NodeAdjacencyFunction* F, const int& cIndex1, const int& cIndex2) + { + F->Function (&children[cIndex1], node); + F->Function (&children[cIndex2], node); + if (children[cIndex1].children) + { + children[cIndex1].__processNodeEdges (node, F, cIndex1, cIndex2); + } + if (children[cIndex2].children) + { + children[cIndex2].__processNodeEdges (node, F, cIndex1, cIndex2); + } } - template + + template template - void OctNode::__processNodeFaces(OctNode* node,NodeAdjacencyFunction* F,const int& cIndex1,const int& cIndex2,const int& cIndex3,const int& cIndex4){ - F->Function(&children[cIndex1],node); - F->Function(&children[cIndex2],node); - F->Function(&children[cIndex3],node); - F->Function(&children[cIndex4],node); - if(children[cIndex1].children){children[cIndex1].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} - if(children[cIndex2].children){children[cIndex2].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} - if(children[cIndex3].children){children[cIndex3].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} - if(children[cIndex4].children){children[cIndex4].__processNodeFaces(node,F,cIndex1,cIndex2,cIndex3,cIndex4);} - } - template + void OctNode + ::__processNodeFaces (OctNode* node, NodeAdjacencyFunction* F, const int& cIndex1, const int& cIndex2, const int& cIndex3, const int& cIndex4) + { + F->Function (&children[cIndex1], node); + F->Function (&children[cIndex2], node); + F->Function (&children[cIndex3], node); + F->Function (&children[cIndex4], node); + if (children[cIndex1].children) + { + children[cIndex1].__processNodeFaces (node, F, cIndex1, cIndex2, cIndex3, cIndex4); + } + if (children[cIndex2].children) + { + children[cIndex2].__processNodeFaces (node, F, cIndex1, cIndex2, cIndex3, cIndex4); + } + if (children[cIndex3].children) + { + children[cIndex3].__processNodeFaces (node, F, cIndex1, cIndex2, cIndex3, cIndex4); + } + if (children[cIndex4].children) + { + children[cIndex4].__processNodeFaces (node, F, cIndex1, cIndex2, cIndex3, cIndex4); + } + } + + template template - void OctNode::ProcessNodeAdjacentNodes(const int& maxDepth,OctNode* node1,const int& width1,OctNode* node2,const int& width2,NodeAdjacencyFunction* F,const int& processCurrent){ - int c1[3],c2[3],w1,w2; - node1->centerIndex(maxDepth+1,c1); - node2->centerIndex(maxDepth+1,c2); - w1=node1->width(maxDepth+1); - w2=node2->width(maxDepth+1); + void OctNode + ::ProcessNodeAdjacentNodes (const int& maxDepth, OctNode* node1, const int& width1, OctNode* node2, const int& width2, NodeAdjacencyFunction* F, const int& processCurrent) + { + int c1[3], c2[3], w1, w2; + node1->centerIndex (maxDepth + 1, c1); + node2->centerIndex (maxDepth + 1, c2); + w1 = node1->width (maxDepth + 1); + w2 = node2->width (maxDepth + 1); - ProcessNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent); + ProcessNodeAdjacentNodes (c1[0] - c2[0], c1[1] - c2[1], c1[2] - c2[2], node1, (width1 * w1) >> 1, node2, (width2 * w2) >> 1, w2, F, processCurrent); } - template + + template template - void OctNode::ProcessNodeAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node1,const int& radius1, - OctNode* node2,const int& radius2,const int& width2, - NodeAdjacencyFunction* F,const int& processCurrent){ - if(!Overlap(dx,dy,dz,radius1+radius2)){return;} - if(processCurrent){F->Function(node2,node1);} - if(!node2->children){return;} - __ProcessNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F); - } - template + void OctNode + ::ProcessNodeAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node1, const int& radius1, + OctNode* node2, const int& radius2, const int& width2, + NodeAdjacencyFunction* F, const int& processCurrent) + { + if (!Overlap (dx, dy, dz, radius1 + radius2)) + { + return; + } + if (processCurrent) + { + F->Function (node2, node1); + } + if (!node2->children) + { + return; + } + __ProcessNodeAdjacentNodes (-dx, -dy, -dz, node1, radius1, node2, radius2, width2 / 2, F); + } + + template template - void OctNode::ProcessTerminatingNodeAdjacentNodes(const int& maxDepth,OctNode* node1,const int& width1,OctNode* node2,const int& width2,TerminatingNodeAdjacencyFunction* F,const int& processCurrent){ - int c1[3],c2[3],w1,w2; - node1->centerIndex(maxDepth+1,c1); - node2->centerIndex(maxDepth+1,c2); - w1=node1->width(maxDepth+1); - w2=node2->width(maxDepth+1); + void OctNode + ::ProcessTerminatingNodeAdjacentNodes (const int& maxDepth, OctNode* node1, const int& width1, OctNode* node2, const int& width2, TerminatingNodeAdjacencyFunction* F, const int& processCurrent) + { + int c1[3], c2[3], w1, w2; + node1->centerIndex (maxDepth + 1, c1); + node2->centerIndex (maxDepth + 1, c2); + w1 = node1->width (maxDepth + 1); + w2 = node2->width (maxDepth + 1); - ProcessTerminatingNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,F,processCurrent); + ProcessTerminatingNodeAdjacentNodes (c1[0] - c2[0], c1[1] - c2[1], c1[2] - c2[2], node1, (width1 * w1) >> 1, node2, (width2 * w2) >> 1, w2, F, processCurrent); } - template + + template template - void OctNode::ProcessTerminatingNodeAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node1,const int& radius1, - OctNode* node2,const int& radius2,const int& width2, - TerminatingNodeAdjacencyFunction* F,const int& processCurrent) - { - if(!Overlap(dx,dy,dz,radius1+radius2)){return;} - if(processCurrent){F->Function(node2,node1);} - if(!node2->children){return;} - __ProcessTerminatingNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,F); - } - template + void OctNode + ::ProcessTerminatingNodeAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node1, const int& radius1, + OctNode* node2, const int& radius2, const int& width2, + TerminatingNodeAdjacencyFunction* F, const int& processCurrent) + { + if (!Overlap (dx, dy, dz, radius1 + radius2)) + { + return; + } + if (processCurrent) + { + F->Function (node2, node1); + } + if (!node2->children) + { + return; + } + __ProcessTerminatingNodeAdjacentNodes (-dx, -dy, -dz, node1, radius1, node2, radius2, width2 / 2, F); + } + + template template - void OctNode::ProcessPointAdjacentNodes(const int& maxDepth,const int c1[3],OctNode* node2,const int& width2,PointAdjacencyFunction* F,const int& processCurrent){ - int c2[3],w2; - node2->centerIndex(maxDepth+1,c2); - w2=node2->width(maxDepth+1); - ProcessPointAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node2,(width2*w2)>>1,w2,F,processCurrent); + void OctNode + ::ProcessPointAdjacentNodes (const int& maxDepth, const int c1[3], OctNode* node2, const int& width2, PointAdjacencyFunction* F, const int& processCurrent) + { + int c2[3], w2; + node2->centerIndex (maxDepth + 1, c2); + w2 = node2->width (maxDepth + 1); + ProcessPointAdjacentNodes (c1[0] - c2[0], c1[1] - c2[1], c1[2] - c2[2], node2, (width2 * w2) >> 1, w2, F, processCurrent); } - template + + template template - void OctNode::ProcessPointAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node2,const int& radius2,const int& width2, - PointAdjacencyFunction* F,const int& processCurrent) - { - if(!Overlap(dx,dy,dz,radius2)){return;} - if(processCurrent){F->Function(node2);} - if(!node2->children){return;} - __ProcessPointAdjacentNodes(-dx,-dy,-dz,node2,radius2,width2>>1,F); - } - template - template - void OctNode::ProcessFixedDepthNodeAdjacentNodes(const int& maxDepth, - OctNode* node1,const int& width1, - OctNode* node2,const int& width2, - const int& depth,NodeAdjacencyFunction* F,const int& processCurrent) - { - int c1[3],c2[3],w1,w2; - node1->centerIndex(maxDepth+1,c1); - node2->centerIndex(maxDepth+1,c2); - w1=node1->width(maxDepth+1); - w2=node2->width(maxDepth+1); - - ProcessFixedDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent); - } - template - template - void OctNode::ProcessFixedDepthNodeAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node1,const int& radius1, - OctNode* node2,const int& radius2,const int& width2, - const int& depth,NodeAdjacencyFunction* F,const int& processCurrent) - { - int d=node2->depth(); - if(d>depth){return;} - if(!Overlap(dx,dy,dz,radius1+radius2)){return;} - if(d==depth){if(processCurrent){F->Function(node2,node1);}} - else{ - if(!node2->children){return;} - __ProcessFixedDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2/2,depth-1,F); - } - } - template - template - void OctNode::ProcessMaxDepthNodeAdjacentNodes(const int& maxDepth, - OctNode* node1,const int& width1, - OctNode* node2,const int& width2, - const int& depth,NodeAdjacencyFunction* F,const int& processCurrent) - { - int c1[3],c2[3],w1,w2; - node1->centerIndex(maxDepth+1,c1); - node2->centerIndex(maxDepth+1,c2); - w1=node1->width(maxDepth+1); - w2=node2->width(maxDepth+1); - ProcessMaxDepthNodeAdjacentNodes(c1[0]-c2[0],c1[1]-c2[1],c1[2]-c2[2],node1,(width1*w1)>>1,node2,(width2*w2)>>1,w2,depth,F,processCurrent); - } - template + void OctNode + ::ProcessPointAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node2, const int& radius2, const int& width2, + PointAdjacencyFunction* F, const int& processCurrent) + { + if (!Overlap (dx, dy, dz, radius2)) + { + return; + } + if (processCurrent) + { + F->Function (node2); + } + if (!node2->children) + { + return; + } + __ProcessPointAdjacentNodes (-dx, -dy, -dz, node2, radius2, width2 >> 1, F); + } + + template template - void OctNode::ProcessMaxDepthNodeAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node1,const int& radius1, - OctNode* node2,const int& radius2,const int& width2, - const int& depth,NodeAdjacencyFunction* F,const int& processCurrent) - { - int d=node2->depth(); - if(d>depth){return;} - if(!Overlap(dx,dy,dz,radius1+radius2)){return;} - if(processCurrent){F->Function(node2,node1);} - if(dchildren){__ProcessMaxDepthNodeAdjacentNodes(-dx,-dy,-dz,node1,radius1,node2,radius2,width2>>1,depth-1,F);} - } - template + void OctNode + ::ProcessFixedDepthNodeAdjacentNodes (const int& maxDepth, + OctNode* node1, const int& width1, + OctNode* node2, const int& width2, + const int& depth, NodeAdjacencyFunction* F, const int& processCurrent) + { + int c1[3], c2[3], w1, w2; + node1->centerIndex (maxDepth + 1, c1); + node2->centerIndex (maxDepth + 1, c2); + w1 = node1->width (maxDepth + 1); + w2 = node2->width (maxDepth + 1); + + ProcessFixedDepthNodeAdjacentNodes (c1[0] - c2[0], c1[1] - c2[1], c1[2] - c2[2], node1, (width1 * w1) >> 1, node2, (width2 * w2) >> 1, w2, depth, F, processCurrent); + } + + template template - void OctNode::__ProcessNodeAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node1,const int& radius1, - OctNode* node2,const int& radius2,const int& cWidth2, - NodeAdjacencyFunction* F) - { - int cWidth=cWidth2>>1; - int radius=radius2>>1; - int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); - if(o){ - int dx1=dx-cWidth; - int dx2=dx+cWidth; - int dy1=dy-cWidth; - int dy2=dy+cWidth; - int dz1=dz-cWidth; - int dz2=dz+cWidth; - if(o& 1){F->Function(&node2->children[0],node1);if(node2->children[0].children){__ProcessNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,F);}} - if(o& 2){F->Function(&node2->children[1],node1);if(node2->children[1].children){__ProcessNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,F);}} - if(o& 4){F->Function(&node2->children[2],node1);if(node2->children[2].children){__ProcessNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,F);}} - if(o& 8){F->Function(&node2->children[3],node1);if(node2->children[3].children){__ProcessNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,F);}} - if(o& 16){F->Function(&node2->children[4],node1);if(node2->children[4].children){__ProcessNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,F);}} - if(o& 32){F->Function(&node2->children[5],node1);if(node2->children[5].children){__ProcessNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,F);}} - if(o& 64){F->Function(&node2->children[6],node1);if(node2->children[6].children){__ProcessNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,F);}} - if(o&128){F->Function(&node2->children[7],node1);if(node2->children[7].children){__ProcessNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}} - } - } - template - template - void OctNode::__ProcessTerminatingNodeAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node1,const int& radius1, - OctNode* node2,const int& radius2,const int& cWidth2, - TerminatingNodeAdjacencyFunction* F) - { - int cWidth=cWidth2>>1; - int radius=radius2>>1; - int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); - if(o){ - int dx1=dx-cWidth; - int dx2=dx+cWidth; - int dy1=dy-cWidth; - int dy2=dy+cWidth; - int dz1=dz-cWidth; - int dz2=dz+cWidth; - if(o& 1){if(F->Function(&node2->children[0],node1) && node2->children[0].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,F);}} - if(o& 2){if(F->Function(&node2->children[1],node1) && node2->children[1].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,F);}} - if(o& 4){if(F->Function(&node2->children[2],node1) && node2->children[2].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,F);}} - if(o& 8){if(F->Function(&node2->children[3],node1) && node2->children[3].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,F);}} - if(o& 16){if(F->Function(&node2->children[4],node1) && node2->children[4].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,F);}} - if(o& 32){if(F->Function(&node2->children[5],node1) && node2->children[5].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,F);}} - if(o& 64){if(F->Function(&node2->children[6],node1) && node2->children[6].children){__ProcessTerminatingNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,F);}} - if(o&128){if(F->Function(&node2->children[7],node1) && node2->children[7].children){__ProcessTerminatingNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,F);}} - } - } - template - template - void OctNode::__ProcessPointAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node2,const int& radius2,const int& cWidth2, - PointAdjacencyFunction* F) - { - int cWidth=cWidth2>>1; - int radius=radius2>>1; - int o=ChildOverlap(dx,dy,dz,radius,cWidth); - if(o){ - int dx1=dx-cWidth; - int dx2=dx+cWidth; - int dy1=dy-cWidth; - int dy2=dy+cWidth; - int dz1=dz-cWidth; - int dz2=dz+cWidth; - if(o& 1){F->Function(&node2->children[0]);if(node2->children[0].children){__ProcessPointAdjacentNodes(dx1,dy1,dz1,&node2->children[0],radius,cWidth,F);}} - if(o& 2){F->Function(&node2->children[1]);if(node2->children[1].children){__ProcessPointAdjacentNodes(dx2,dy1,dz1,&node2->children[1],radius,cWidth,F);}} - if(o& 4){F->Function(&node2->children[2]);if(node2->children[2].children){__ProcessPointAdjacentNodes(dx1,dy2,dz1,&node2->children[2],radius,cWidth,F);}} - if(o& 8){F->Function(&node2->children[3]);if(node2->children[3].children){__ProcessPointAdjacentNodes(dx2,dy2,dz1,&node2->children[3],radius,cWidth,F);}} - if(o& 16){F->Function(&node2->children[4]);if(node2->children[4].children){__ProcessPointAdjacentNodes(dx1,dy1,dz2,&node2->children[4],radius,cWidth,F);}} - if(o& 32){F->Function(&node2->children[5]);if(node2->children[5].children){__ProcessPointAdjacentNodes(dx2,dy1,dz2,&node2->children[5],radius,cWidth,F);}} - if(o& 64){F->Function(&node2->children[6]);if(node2->children[6].children){__ProcessPointAdjacentNodes(dx1,dy2,dz2,&node2->children[6],radius,cWidth,F);}} - if(o&128){F->Function(&node2->children[7]);if(node2->children[7].children){__ProcessPointAdjacentNodes(dx2,dy2,dz2,&node2->children[7],radius,cWidth,F);}} - } - } - template + void OctNode + ::ProcessFixedDepthNodeAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node1, const int& radius1, + OctNode* node2, const int& radius2, const int& width2, + const int& depth, NodeAdjacencyFunction* F, const int& processCurrent) + { + int d = node2->depth (); + if (d > depth) + { + return; + } + if (!Overlap (dx, dy, dz, radius1 + radius2)) + { + return; + } + if (d == depth) + { + if (processCurrent) + { + F->Function (node2, node1); + } + } + else + { + if (!node2->children) + { + return; + } + __ProcessFixedDepthNodeAdjacentNodes (-dx, -dy, -dz, node1, radius1, node2, radius2, width2 / 2, depth - 1, F); + } + } + + template template - void OctNode::__ProcessFixedDepthNodeAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node1,const int& radius1, - OctNode* node2,const int& radius2,const int& cWidth2, - const int& depth,NodeAdjacencyFunction* F) - { - int cWidth=cWidth2>>1; - int radius=radius2>>1; - int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); - if(o){ - int dx1=dx-cWidth; - int dx2=dx+cWidth; - int dy1=dy-cWidth; - int dy2=dy+cWidth; - int dz1=dz-cWidth; - int dz2=dz+cWidth; - if(node2->depth()==depth){ - if(o& 1){F->Function(&node2->children[0],node1);} - if(o& 2){F->Function(&node2->children[1],node1);} - if(o& 4){F->Function(&node2->children[2],node1);} - if(o& 8){F->Function(&node2->children[3],node1);} - if(o& 16){F->Function(&node2->children[4],node1);} - if(o& 32){F->Function(&node2->children[5],node1);} - if(o& 64){F->Function(&node2->children[6],node1);} - if(o&128){F->Function(&node2->children[7],node1);} - } - else{ - if(o& 1){if(node2->children[0].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,depth,F);}} - if(o& 2){if(node2->children[1].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,depth,F);}} - if(o& 4){if(node2->children[2].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,depth,F);}} - if(o& 8){if(node2->children[3].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,depth,F);}} - if(o& 16){if(node2->children[4].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,depth,F);}} - if(o& 32){if(node2->children[5].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,depth,F);}} - if(o& 64){if(node2->children[6].children){__ProcessFixedDepthNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,depth,F);}} - if(o&128){if(node2->children[7].children){__ProcessFixedDepthNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,depth,F);}} - } - } - } - template + void OctNode + ::ProcessMaxDepthNodeAdjacentNodes (const int& maxDepth, + OctNode* node1, const int& width1, + OctNode* node2, const int& width2, + const int& depth, NodeAdjacencyFunction* F, const int& processCurrent) + { + int c1[3], c2[3], w1, w2; + node1->centerIndex (maxDepth + 1, c1); + node2->centerIndex (maxDepth + 1, c2); + w1 = node1->width (maxDepth + 1); + w2 = node2->width (maxDepth + 1); + ProcessMaxDepthNodeAdjacentNodes (c1[0] - c2[0], c1[1] - c2[1], c1[2] - c2[2], node1, (width1 * w1) >> 1, node2, (width2 * w2) >> 1, w2, depth, F, processCurrent); + } + + template template - void OctNode::__ProcessMaxDepthNodeAdjacentNodes(const int& dx,const int& dy,const int& dz, - OctNode* node1,const int& radius1, - OctNode* node2,const int& radius2,const int& cWidth2, - const int& depth,NodeAdjacencyFunction* F) - { - int cWidth=cWidth2>>1; - int radius=radius2>>1; - int o=ChildOverlap(dx,dy,dz,radius1+radius,cWidth); - if(o){ - int dx1=dx-cWidth; - int dx2=dx+cWidth; - int dy1=dy-cWidth; - int dy2=dy+cWidth; - int dz1=dz-cWidth; - int dz2=dz+cWidth; - if(node2->depth()<=depth){ - if(o& 1){F->Function(&node2->children[0],node1);} - if(o& 2){F->Function(&node2->children[1],node1);} - if(o& 4){F->Function(&node2->children[2],node1);} - if(o& 8){F->Function(&node2->children[3],node1);} - if(o& 16){F->Function(&node2->children[4],node1);} - if(o& 32){F->Function(&node2->children[5],node1);} - if(o& 64){F->Function(&node2->children[6],node1);} - if(o&128){F->Function(&node2->children[7],node1);} - } - if(node2->depth()children[0].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy1,dz1,node1,radius1,&node2->children[0],radius,cWidth,depth,F);}} - if(o& 2){if(node2->children[1].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy1,dz1,node1,radius1,&node2->children[1],radius,cWidth,depth,F);}} - if(o& 4){if(node2->children[2].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy2,dz1,node1,radius1,&node2->children[2],radius,cWidth,depth,F);}} - if(o& 8){if(node2->children[3].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy2,dz1,node1,radius1,&node2->children[3],radius,cWidth,depth,F);}} - if(o& 16){if(node2->children[4].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy1,dz2,node1,radius1,&node2->children[4],radius,cWidth,depth,F);}} - if(o& 32){if(node2->children[5].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy1,dz2,node1,radius1,&node2->children[5],radius,cWidth,depth,F);}} - if(o& 64){if(node2->children[6].children){__ProcessMaxDepthNodeAdjacentNodes(dx1,dy2,dz2,node1,radius1,&node2->children[6],radius,cWidth,depth,F);}} - if(o&128){if(node2->children[7].children){__ProcessMaxDepthNodeAdjacentNodes(dx2,dy2,dz2,node1,radius1,&node2->children[7],radius,cWidth,depth,F);}} - } - } - } - template - inline int OctNode::ChildOverlap(const int& dx,const int& dy,const int& dz,const int& d,const int& cRadius2) - { - int w1=d-cRadius2; - int w2=d+cRadius2; - int overlap=0; - - int test=0,test1=0; - if(dx-w1){test =1;} - if(dx-w2){test|=2;} - - if(!test){return 0;} - if(dz-w1){test1 =test;} - if(dz-w2){test1|=test<<4;} - - if(!test1){return 0;} - if(dy-w1){overlap =test1;} - if(dy-w2){overlap|=test1<<2;} - return overlap; + void OctNode + ::ProcessMaxDepthNodeAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node1, const int& radius1, + OctNode* node2, const int& radius2, const int& width2, + const int& depth, NodeAdjacencyFunction* F, const int& processCurrent) + { + int d = node2->depth (); + if (d > depth) + { + return; + } + if (!Overlap (dx, dy, dz, radius1 + radius2)) + { + return; + } + if (processCurrent) + { + F->Function (node2, node1); + } + if (d < depth && node2->children) + { + __ProcessMaxDepthNodeAdjacentNodes (-dx, -dy, -dz, node1, radius1, node2, radius2, width2 >> 1, depth - 1, F); + } + } + + template + template + void OctNode + ::__ProcessNodeAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node1, const int& radius1, + OctNode* node2, const int& radius2, const int& cWidth2, + NodeAdjacencyFunction* F) + { + int cWidth = cWidth2 >> 1; + int radius = radius2 >> 1; + int o = ChildOverlap (dx, dy, dz, radius1 + radius, cWidth); + if (o) + { + int dx1 = dx - cWidth; + int dx2 = dx + cWidth; + int dy1 = dy - cWidth; + int dy2 = dy + cWidth; + int dz1 = dz - cWidth; + int dz2 = dz + cWidth; + if (o & 1) + { + F->Function (&node2->children[0], node1); + if (node2->children[0].children) + { + __ProcessNodeAdjacentNodes (dx1, dy1, dz1, node1, radius1, &node2->children[0], radius, cWidth, F); + } + } + if (o & 2) + { + F->Function (&node2->children[1], node1); + if (node2->children[1].children) + { + __ProcessNodeAdjacentNodes (dx2, dy1, dz1, node1, radius1, &node2->children[1], radius, cWidth, F); + } + } + if (o & 4) + { + F->Function (&node2->children[2], node1); + if (node2->children[2].children) + { + __ProcessNodeAdjacentNodes (dx1, dy2, dz1, node1, radius1, &node2->children[2], radius, cWidth, F); + } + } + if (o & 8) + { + F->Function (&node2->children[3], node1); + if (node2->children[3].children) + { + __ProcessNodeAdjacentNodes (dx2, dy2, dz1, node1, radius1, &node2->children[3], radius, cWidth, F); + } + } + if (o & 16) + { + F->Function (&node2->children[4], node1); + if (node2->children[4].children) + { + __ProcessNodeAdjacentNodes (dx1, dy1, dz2, node1, radius1, &node2->children[4], radius, cWidth, F); + } + } + if (o & 32) + { + F->Function (&node2->children[5], node1); + if (node2->children[5].children) + { + __ProcessNodeAdjacentNodes (dx2, dy1, dz2, node1, radius1, &node2->children[5], radius, cWidth, F); + } + } + if (o & 64) + { + F->Function (&node2->children[6], node1); + if (node2->children[6].children) + { + __ProcessNodeAdjacentNodes (dx1, dy2, dz2, node1, radius1, &node2->children[6], radius, cWidth, F); + } + } + if (o & 128) + { + F->Function (&node2->children[7], node1); + if (node2->children[7].children) + { + __ProcessNodeAdjacentNodes (dx2, dy2, dz2, node1, radius1, &node2->children[7], radius, cWidth, F); + } + } + } + } + + template + template + void OctNode + ::__ProcessTerminatingNodeAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node1, const int& radius1, + OctNode* node2, const int& radius2, const int& cWidth2, + TerminatingNodeAdjacencyFunction* F) + { + int cWidth = cWidth2 >> 1; + int radius = radius2 >> 1; + int o = ChildOverlap (dx, dy, dz, radius1 + radius, cWidth); + if (o) + { + int dx1 = dx - cWidth; + int dx2 = dx + cWidth; + int dy1 = dy - cWidth; + int dy2 = dy + cWidth; + int dz1 = dz - cWidth; + int dz2 = dz + cWidth; + if (o & 1) + { + if (F->Function (&node2->children[0], node1) && node2->children[0].children) + { + __ProcessTerminatingNodeAdjacentNodes (dx1, dy1, dz1, node1, radius1, &node2->children[0], radius, cWidth, F); + } + } + if (o & 2) + { + if (F->Function (&node2->children[1], node1) && node2->children[1].children) + { + __ProcessTerminatingNodeAdjacentNodes (dx2, dy1, dz1, node1, radius1, &node2->children[1], radius, cWidth, F); + } + } + if (o & 4) + { + if (F->Function (&node2->children[2], node1) && node2->children[2].children) + { + __ProcessTerminatingNodeAdjacentNodes (dx1, dy2, dz1, node1, radius1, &node2->children[2], radius, cWidth, F); + } + } + if (o & 8) + { + if (F->Function (&node2->children[3], node1) && node2->children[3].children) + { + __ProcessTerminatingNodeAdjacentNodes (dx2, dy2, dz1, node1, radius1, &node2->children[3], radius, cWidth, F); + } + } + if (o & 16) + { + if (F->Function (&node2->children[4], node1) && node2->children[4].children) + { + __ProcessTerminatingNodeAdjacentNodes (dx1, dy1, dz2, node1, radius1, &node2->children[4], radius, cWidth, F); + } + } + if (o & 32) + { + if (F->Function (&node2->children[5], node1) && node2->children[5].children) + { + __ProcessTerminatingNodeAdjacentNodes (dx2, dy1, dz2, node1, radius1, &node2->children[5], radius, cWidth, F); + } + } + if (o & 64) + { + if (F->Function (&node2->children[6], node1) && node2->children[6].children) + { + __ProcessTerminatingNodeAdjacentNodes (dx1, dy2, dz2, node1, radius1, &node2->children[6], radius, cWidth, F); + } + } + if (o & 128) + { + if (F->Function (&node2->children[7], node1) && node2->children[7].children) + { + __ProcessTerminatingNodeAdjacentNodes (dx2, dy2, dz2, node1, radius1, &node2->children[7], radius, cWidth, F); + } + } + } + } + + template + template + void OctNode + ::__ProcessPointAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node2, const int& radius2, const int& cWidth2, + PointAdjacencyFunction* F) + { + int cWidth = cWidth2 >> 1; + int radius = radius2 >> 1; + int o = ChildOverlap (dx, dy, dz, radius, cWidth); + if (o) + { + int dx1 = dx - cWidth; + int dx2 = dx + cWidth; + int dy1 = dy - cWidth; + int dy2 = dy + cWidth; + int dz1 = dz - cWidth; + int dz2 = dz + cWidth; + if (o & 1) + { + F->Function (&node2->children[0]); + if (node2->children[0].children) + { + __ProcessPointAdjacentNodes (dx1, dy1, dz1, &node2->children[0], radius, cWidth, F); + } + } + if (o & 2) + { + F->Function (&node2->children[1]); + if (node2->children[1].children) + { + __ProcessPointAdjacentNodes (dx2, dy1, dz1, &node2->children[1], radius, cWidth, F); + } + } + if (o & 4) + { + F->Function (&node2->children[2]); + if (node2->children[2].children) + { + __ProcessPointAdjacentNodes (dx1, dy2, dz1, &node2->children[2], radius, cWidth, F); + } + } + if (o & 8) + { + F->Function (&node2->children[3]); + if (node2->children[3].children) + { + __ProcessPointAdjacentNodes (dx2, dy2, dz1, &node2->children[3], radius, cWidth, F); + } + } + if (o & 16) + { + F->Function (&node2->children[4]); + if (node2->children[4].children) + { + __ProcessPointAdjacentNodes (dx1, dy1, dz2, &node2->children[4], radius, cWidth, F); + } + } + if (o & 32) + { + F->Function (&node2->children[5]); + if (node2->children[5].children) + { + __ProcessPointAdjacentNodes (dx2, dy1, dz2, &node2->children[5], radius, cWidth, F); + } + } + if (o & 64) + { + F->Function (&node2->children[6]); + if (node2->children[6].children) + { + __ProcessPointAdjacentNodes (dx1, dy2, dz2, &node2->children[6], radius, cWidth, F); + } + } + if (o & 128) + { + F->Function (&node2->children[7]); + if (node2->children[7].children) + { + __ProcessPointAdjacentNodes (dx2, dy2, dz2, &node2->children[7], radius, cWidth, F); + } + } + } + } + + template + template + void OctNode + ::__ProcessFixedDepthNodeAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node1, const int& radius1, + OctNode* node2, const int& radius2, const int& cWidth2, + const int& depth, NodeAdjacencyFunction* F) + { + int cWidth = cWidth2 >> 1; + int radius = radius2 >> 1; + int o = ChildOverlap (dx, dy, dz, radius1 + radius, cWidth); + if (o) + { + int dx1 = dx - cWidth; + int dx2 = dx + cWidth; + int dy1 = dy - cWidth; + int dy2 = dy + cWidth; + int dz1 = dz - cWidth; + int dz2 = dz + cWidth; + if (node2->depth () == depth) + { + if (o & 1) + { + F->Function (&node2->children[0], node1); + } + if (o & 2) + { + F->Function (&node2->children[1], node1); + } + if (o & 4) + { + F->Function (&node2->children[2], node1); + } + if (o & 8) + { + F->Function (&node2->children[3], node1); + } + if (o & 16) + { + F->Function (&node2->children[4], node1); + } + if (o & 32) + { + F->Function (&node2->children[5], node1); + } + if (o & 64) + { + F->Function (&node2->children[6], node1); + } + if (o & 128) + { + F->Function (&node2->children[7], node1); + } + } + else + { + if (o & 1) + { + if (node2->children[0].children) + { + __ProcessFixedDepthNodeAdjacentNodes (dx1, dy1, dz1, node1, radius1, &node2->children[0], radius, cWidth, depth, F); + } + } + if (o & 2) + { + if (node2->children[1].children) + { + __ProcessFixedDepthNodeAdjacentNodes (dx2, dy1, dz1, node1, radius1, &node2->children[1], radius, cWidth, depth, F); + } + } + if (o & 4) + { + if (node2->children[2].children) + { + __ProcessFixedDepthNodeAdjacentNodes (dx1, dy2, dz1, node1, radius1, &node2->children[2], radius, cWidth, depth, F); + } + } + if (o & 8) + { + if (node2->children[3].children) + { + __ProcessFixedDepthNodeAdjacentNodes (dx2, dy2, dz1, node1, radius1, &node2->children[3], radius, cWidth, depth, F); + } + } + if (o & 16) + { + if (node2->children[4].children) + { + __ProcessFixedDepthNodeAdjacentNodes (dx1, dy1, dz2, node1, radius1, &node2->children[4], radius, cWidth, depth, F); + } + } + if (o & 32) + { + if (node2->children[5].children) + { + __ProcessFixedDepthNodeAdjacentNodes (dx2, dy1, dz2, node1, radius1, &node2->children[5], radius, cWidth, depth, F); + } + } + if (o & 64) + { + if (node2->children[6].children) + { + __ProcessFixedDepthNodeAdjacentNodes (dx1, dy2, dz2, node1, radius1, &node2->children[6], radius, cWidth, depth, F); + } + } + if (o & 128) + { + if (node2->children[7].children) + { + __ProcessFixedDepthNodeAdjacentNodes (dx2, dy2, dz2, node1, radius1, &node2->children[7], radius, cWidth, depth, F); + } + } + } + } + } + + template + template + void OctNode + ::__ProcessMaxDepthNodeAdjacentNodes (const int& dx, const int& dy, const int& dz, + OctNode* node1, const int& radius1, + OctNode* node2, const int& radius2, const int& cWidth2, + const int& depth, NodeAdjacencyFunction* F) + { + int cWidth = cWidth2 >> 1; + int radius = radius2 >> 1; + int o = ChildOverlap (dx, dy, dz, radius1 + radius, cWidth); + if (o) + { + int dx1 = dx - cWidth; + int dx2 = dx + cWidth; + int dy1 = dy - cWidth; + int dy2 = dy + cWidth; + int dz1 = dz - cWidth; + int dz2 = dz + cWidth; + if (node2->depth () <= depth) + { + if (o & 1) + { + F->Function (&node2->children[0], node1); + } + if (o & 2) + { + F->Function (&node2->children[1], node1); + } + if (o & 4) + { + F->Function (&node2->children[2], node1); + } + if (o & 8) + { + F->Function (&node2->children[3], node1); + } + if (o & 16) + { + F->Function (&node2->children[4], node1); + } + if (o & 32) + { + F->Function (&node2->children[5], node1); + } + if (o & 64) + { + F->Function (&node2->children[6], node1); + } + if (o & 128) + { + F->Function (&node2->children[7], node1); + } + } + if (node2->depth () < depth) + { + if (o & 1) + { + if (node2->children[0].children) + { + __ProcessMaxDepthNodeAdjacentNodes (dx1, dy1, dz1, node1, radius1, &node2->children[0], radius, cWidth, depth, F); + } + } + if (o & 2) + { + if (node2->children[1].children) + { + __ProcessMaxDepthNodeAdjacentNodes (dx2, dy1, dz1, node1, radius1, &node2->children[1], radius, cWidth, depth, F); + } + } + if (o & 4) + { + if (node2->children[2].children) + { + __ProcessMaxDepthNodeAdjacentNodes (dx1, dy2, dz1, node1, radius1, &node2->children[2], radius, cWidth, depth, F); + } + } + if (o & 8) + { + if (node2->children[3].children) + { + __ProcessMaxDepthNodeAdjacentNodes (dx2, dy2, dz1, node1, radius1, &node2->children[3], radius, cWidth, depth, F); + } + } + if (o & 16) + { + if (node2->children[4].children) + { + __ProcessMaxDepthNodeAdjacentNodes (dx1, dy1, dz2, node1, radius1, &node2->children[4], radius, cWidth, depth, F); + } + } + if (o & 32) + { + if (node2->children[5].children) + { + __ProcessMaxDepthNodeAdjacentNodes (dx2, dy1, dz2, node1, radius1, &node2->children[5], radius, cWidth, depth, F); + } + } + if (o & 64) + { + if (node2->children[6].children) + { + __ProcessMaxDepthNodeAdjacentNodes (dx1, dy2, dz2, node1, radius1, &node2->children[6], radius, cWidth, depth, F); + } + } + if (o & 128) + { + if (node2->children[7].children) + { + __ProcessMaxDepthNodeAdjacentNodes (dx2, dy2, dz2, node1, radius1, &node2->children[7], radius, cWidth, depth, F); + } + } + } + } + } + + template + inline int OctNode + ::ChildOverlap (const int& dx, const int& dy, const int& dz, const int& d, const int& cRadius2) + { + int w1 = d - cRadius2; + int w2 = d + cRadius2; + int overlap = 0; + + int test = 0, test1 = 0; + if (dx < w2 && dx>-w1) + { + test = 1; + } + if (dx < w1 && dx>-w2) + { + test |= 2; + } + + if (!test) + { + return 0; + } + if (dz < w2 && dz>-w1) + { + test1 = test; + } + if (dz < w1 && dz>-w2) + { + test1 |= test << 4; + } + + if (!test1) + { + return 0; + } + if (dy < w2 && dy>-w1) + { + overlap = test1; + } + if (dy < w1 && dy>-w2) + { + overlap |= test1 << 2; + } + return overlap; + } + + template + OctNode* OctNode + ::getNearestLeaf (const Point3D& p) + { + Point3D center; + Real width; + OctNode* temp; + int cIndex; + if (!children) + { + return this; + } + centerAndWidth (center, width); + temp = this; + while (temp->children) + { + cIndex = CornerIndex (center, p); + temp = &temp->children[cIndex]; + width /= 2; + if (cIndex & 1) + { + center.coords[0] += width / 2; + } + else + { + center.coords[0] -= width / 2; + } + if (cIndex & 2) + { + center.coords[1] += width / 2; + } + else + { + center.coords[1] -= width / 2; + } + if (cIndex & 4) + { + center.coords[2] += width / 2; + } + else + { + center.coords[2] -= width / 2; + } + } + return temp; + } + + template + const OctNode* OctNode + ::getNearestLeaf (const Point3D& p) const + { + int nearest; + Real temp, dist2; + if (!children) + { + return this; + } + for (int i = 0; i < Cube::CORNERS; i++) + { + temp = SquareDistance (children[i].center, p); + if (!i || temp < dist2) + { + dist2 = temp; + nearest = i; + } + } + return children[nearest].getNearestLeaf (p); + } + + template + int OctNode + ::CommonEdge (const OctNode* node1, const int& eIndex1, const OctNode* node2, const int& eIndex2) + { + int o1, o2, i1, i2, j1, j2; + + Cube::FactorEdgeIndex (eIndex1, o1, i1, j1); + Cube::FactorEdgeIndex (eIndex2, o2, i2, j2); + if (o1 != o2) + { + return 0; + } + + int dir[2]; + int idx1[2]; + int idx2[2]; + switch (o1) + { + case 0: dir[0] = 1; + dir[1] = 2; + break; + case 1: dir[0] = 0; + dir[1] = 2; + break; + case 2: dir[0] = 0; + dir[1] = 1; + break; + }; + int d1, d2, off1[3], off2[3]; + node1->depthAndOffset (d1, off1); + node2->depthAndOffset (d2, off2); + idx1[0] = off1[dir[0]]+(1 << d1) + i1; + idx1[1] = off1[dir[1]]+(1 << d1) + j1; + idx2[0] = off2[dir[0]]+(1 << d2) + i2; + idx2[1] = off2[dir[1]]+(1 << d2) + j2; + if (d1 > d2) + { + idx2[0] <<= (d1 - d2); + idx2[1] <<= (d1 - d2); + } + else + { + idx1[0] <<= (d2 - d1); + idx1[1] <<= (d2 - d1); + } + if (idx1[0] == idx2[0] && idx1[1] == idx2[1]) + { + return 1; + } + else + { + return 0; + } + } + + template + int OctNode + ::CornerIndex (const Point3D& center, const Point3D& p) + { + int cIndex = 0; + if (p.coords[0] > center.coords[0]) + { + cIndex |= 1; + } + if (p.coords[1] > center.coords[1]) + { + cIndex |= 2; + } + if (p.coords[2] > center.coords[2]) + { + cIndex |= 4; + } + return cIndex; + } + + template + template + OctNode& OctNode::operator = (const OctNode& node) + { + int i; + if (children) + { + delete[] children; + } + children = NULL; + + depth = node.depth; + for (i = 0; i < DIMENSION; i++) + { + this->offset[i] = node.offset[i]; + } + if (node.children) + { + initChildren (); + for (i = 0; i < Cube::CORNERS; i++) + { + children[i] = node.children[i]; + } + } + return *this; + } + + template + int OctNode + ::CompareForwardDepths (const void* v1, const void* v2) + { + return ((const OctNode*)v1)->depth - ((const OctNode*)v2)->depth; + } + + template + int OctNode + ::CompareForwardPointerDepths (const void* v1, const void* v2) + { + const OctNode *n1, *n2; + n1 = (*(const OctNode**)v1); + n2 = (*(const OctNode**)v2); + if (n1->d != n2->d) + { + return int(n1->d) - int(n2->d); + } + while (n1->parent != n2->parent) + { + n1 = n1->parent; + n2 = n2->parent; + } + if (n1->off[0] != n2->off[0]) + { + return int(n1->off[0]) - int(n2->off[0]); + } + if (n1->off[1] != n2->off[1]) + { + return int(n1->off[1]) - int(n2->off[1]); + } + return int(n1->off[2]) - int(n2->off[2]); + return 0; + } + + template + int OctNode + ::CompareBackwardDepths (const void* v1, const void* v2) + { + return ((const OctNode*)v2)->depth - ((const OctNode*)v1)->depth; + } + + template + int OctNode + ::CompareBackwardPointerDepths (const void* v1, const void* v2) + { + return (*(const OctNode**)v2)->depth ()-(*(const OctNode**)v1)->depth (); + } + + template + inline int OctNode + ::Overlap2 (const int &depth1, const int offSet1[DIMENSION], const Real& multiplier1, const int &depth2, const int offSet2[DIMENSION], const Real& multiplier2) + { + int d = depth2 - depth1; + Real w = multiplier2 + multiplier1 * (1 << d); + Real w2 = Real ((1 << (d - 1)) - 0.5); + if ( + fabs (Real (offSet2[0]-(offSet1[0] << d)) - w2) >= w || + fabs (Real (offSet2[1]-(offSet1[1] << d)) - w2) >= w || + fabs (Real (offSet2[2]-(offSet1[2] << d)) - w2) >= w + ) + { + return 0; + } + return 1; + } + + template + inline int OctNode + ::Overlap (const int& c1, const int& c2, const int& c3, const int& dWidth) + { + if (c1 >= dWidth || c1 <= -dWidth || c2 >= dWidth || c2 <= -dWidth || c3 >= dWidth || c3 <= -dWidth) + { + return 0; + } + else + { + return 1; + } } - template - OctNode* OctNode::getNearestLeaf(const Point3D& p){ - Point3D center; - Real width; - OctNode* temp; - int cIndex; - if(!children){return this;} - centerAndWidth(center,width); - temp=this; - while(temp->children){ - cIndex=CornerIndex(center,p); - temp=&temp->children[cIndex]; - width/=2; - if(cIndex&1){center.coords[0]+=width/2;} - else {center.coords[0]-=width/2;} - if(cIndex&2){center.coords[1]+=width/2;} - else {center.coords[1]-=width/2;} - if(cIndex&4){center.coords[2]+=width/2;} - else {center.coords[2]-=width/2;} - } - return temp; + template + OctNode* OctNode + ::faceNeighbor (const int& faceIndex, const int& forceChildren) + { + return __faceNeighbor (faceIndex >> 1, faceIndex & 1, forceChildren); } - template - const OctNode* OctNode::getNearestLeaf(const Point3D& p) const{ - int nearest; - Real temp,dist2; - if(!children){return this;} - for(int i=0;i + const OctNode* OctNode + ::faceNeighbor (const int& faceIndex) const + { + return __faceNeighbor (faceIndex >> 1, faceIndex & 1); + } + + template + OctNode* OctNode + ::__faceNeighbor (const int& dir, const int& off, const int& forceChildren) + { + if (!parent) + { + return NULL; + } + int pIndex = int(this-parent->children); + pIndex ^= (1 << dir); + if ((pIndex & (1 << dir)) == (off << dir)) + { + return &parent->children[pIndex]; + } + // if(!(((pIndex>>dir)^off)&1)){return &parent->children[pIndex];} + else + { + OctNode* temp = parent->__faceNeighbor (dir, off, forceChildren); + if (!temp) + { + return NULL; } + if (!temp->children) + { + if (forceChildren) + { + temp->initChildren (); + } + else + { + return temp; + } + } + return &temp->children[pIndex]; } - return children[nearest].getNearestLeaf(p); } - template - int OctNode::CommonEdge(const OctNode* node1,const int& eIndex1,const OctNode* node2,const int& eIndex2){ - int o1,o2,i1,i2,j1,j2; + template + const OctNode* OctNode + ::__faceNeighbor (const int& dir, const int& off) const + { + if (!parent) + { + return NULL; + } + int pIndex = int(this-parent->children); + pIndex ^= (1 << dir); + if ((pIndex & (1 << dir)) == (off << dir)) + { + return &parent->children[pIndex]; + } + // if(!(((pIndex>>dir)^off)&1)){return &parent->children[pIndex];} + else + { + const OctNode* temp = parent->__faceNeighbor (dir, off); + if (!temp || !temp->children) + { + return temp; + } + else + { + return &temp->children[pIndex]; + } + } + } - Cube::FactorEdgeIndex(eIndex1,o1,i1,j1); - Cube::FactorEdgeIndex(eIndex2,o2,i2,j2); - if(o1!=o2){return 0;} + template + OctNode* OctNode + ::edgeNeighbor (const int& edgeIndex, const int& forceChildren) + { + int idx[2], o, i[2]; + Cube::FactorEdgeIndex (edgeIndex, o, i[0], i[1]); + switch (o) + { + case 0: idx[0] = 1; + idx[1] = 2; + break; + case 1: idx[0] = 0; + idx[1] = 2; + break; + case 2: idx[0] = 0; + idx[1] = 1; + break; + }; + return __edgeNeighbor (o, i, idx, forceChildren); + } - int dir[2]; - int idx1[2]; - int idx2[2]; - switch(o1){ - case 0: dir[0]=1; dir[1]=2; break; - case 1: dir[0]=0; dir[1]=2; break; - case 2: dir[0]=0; dir[1]=1; break; + template + const OctNode* OctNode + ::edgeNeighbor (const int& edgeIndex) const + { + int idx[2], o, i[2]; + Cube::FactorEdgeIndex (edgeIndex, o, i[0], i[1]); + switch (o) + { + case 0: idx[0] = 1; + idx[1] = 2; + break; + case 1: idx[0] = 0; + idx[1] = 2; + break; + case 2: idx[0] = 0; + idx[1] = 1; + break; }; - int d1,d2,off1[3],off2[3]; - node1->depthAndOffset(d1,off1); - node2->depthAndOffset(d2,off2); - idx1[0]=off1[dir[0]]+(1<d2){ - idx2[0]<<=(d1-d2); - idx2[1]<<=(d1-d2); - } - else{ - idx1[0]<<=(d2-d1); - idx1[1]<<=(d2-d1); - } - if(idx1[0]==idx2[0] && idx1[1]==idx2[1]){return 1;} - else {return 0;} - } - template - int OctNode::CornerIndex(const Point3D& center,const Point3D& p){ - int cIndex=0; - if(p.coords[0]>center.coords[0]){cIndex|=1;} - if(p.coords[1]>center.coords[1]){cIndex|=2;} - if(p.coords[2]>center.coords[2]){cIndex|=4;} - return cIndex; + return __edgeNeighbor (o, i, idx); } - template - template - OctNode& OctNode::operator = (const OctNode& node){ - int i; - if(children){delete[] children;} - children=NULL; - depth=node.depth; - for(i=0;ioffset[i] = node.offset[i];} - if(node.children){ - initChildren(); - for(i=0;i + const OctNode* OctNode + ::__edgeNeighbor (const int& o, const int i[2], const int idx[2]) const + { + if (!parent) + { + return NULL; + } + int pIndex = int(this-parent->children); + int aIndex, x[DIMENSION]; + + Cube::FactorCornerIndex (pIndex, x[0], x[1], x[2]); + aIndex = (~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]]) << 1))) & 3; + pIndex ^= (7 ^ (1 << o)); + if (aIndex == 1) + { // I can get the neighbor from the parent's face adjacent neighbor + const OctNode* temp = parent->__faceNeighbor (idx[0], i[0]); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return &temp->children[pIndex]; + } } - return *this; - } - template - int OctNode::CompareForwardDepths(const void* v1,const void* v2){ - return ((const OctNode*)v1)->depth-((const OctNode*)v2)->depth; - } - template - int OctNode::CompareForwardPointerDepths(const void* v1,const void* v2){ - const OctNode *n1,*n2; - n1=(*(const OctNode**)v1); - n2=(*(const OctNode**)v2); - if(n1->d!=n2->d){return int(n1->d)-int(n2->d);} - while(n1->parent != n2->parent){ - n1=n1->parent; - n2=n2->parent; - } - if(n1->off[0]!=n2->off[0]){return int(n1->off[0])-int(n2->off[0]);} - if(n1->off[1]!=n2->off[1]){return int(n1->off[1])-int(n2->off[1]);} - return int(n1->off[2])-int(n2->off[2]); - return 0; - } - template - int OctNode::CompareBackwardDepths(const void* v1,const void* v2){ - return ((const OctNode*)v2)->depth-((const OctNode*)v1)->depth; - } - template - int OctNode::CompareBackwardPointerDepths(const void* v1,const void* v2){ - return (*(const OctNode**)v2)->depth()-(*(const OctNode**)v1)->depth(); - } - template - inline int OctNode::Overlap2(const int &depth1,const int offSet1[DIMENSION],const Real& multiplier1,const int &depth2,const int offSet2[DIMENSION],const Real& multiplier2){ - int d=depth2-depth1; - Real w=multiplier2+multiplier1*(1<=w || - fabs(Real(offSet2[1]-(offSet1[1]<=w || - fabs(Real(offSet2[2]-(offSet1[2]<=w - ){return 0;} - return 1; - } - template - inline int OctNode::Overlap(const int& c1,const int& c2,const int& c3,const int& dWidth){ - if(c1>=dWidth || c1<=-dWidth || c2>=dWidth || c2<=-dWidth || c3>=dWidth || c3<=-dWidth){return 0;} - else{return 1;} - } - template - OctNode* OctNode::faceNeighbor(const int& faceIndex,const int& forceChildren){return __faceNeighbor(faceIndex>>1,faceIndex&1,forceChildren);} - template - const OctNode* OctNode::faceNeighbor(const int& faceIndex) const {return __faceNeighbor(faceIndex>>1,faceIndex&1);} - template - OctNode* OctNode::__faceNeighbor(const int& dir,const int& off,const int& forceChildren){ - if(!parent){return NULL;} - int pIndex=int(this-parent->children); - pIndex^=(1<children[pIndex];} - // if(!(((pIndex>>dir)^off)&1)){return &parent->children[pIndex];} - else{ - OctNode* temp=parent->__faceNeighbor(dir,off,forceChildren); - if(!temp){return NULL;} - if(!temp->children){ - if(forceChildren){temp->initChildren();} - else{return temp;} + else if (aIndex == 2) + { // I can get the neighbor from the parent's face adjacent neighbor + const OctNode* temp = parent->__faceNeighbor (idx[1], i[1]); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return &temp->children[pIndex]; } - return &temp->children[pIndex]; } - } - template - const OctNode* OctNode::__faceNeighbor(const int& dir,const int& off) const { - if(!parent){return NULL;} - int pIndex=int(this-parent->children); - pIndex^=(1<children[pIndex];} - // if(!(((pIndex>>dir)^off)&1)){return &parent->children[pIndex];} - else{ - const OctNode* temp=parent->__faceNeighbor(dir,off); - if(!temp || !temp->children){return temp;} - else{return &temp->children[pIndex];} - } - } - - template - OctNode* OctNode::edgeNeighbor(const int& edgeIndex,const int& forceChildren){ - int idx[2],o,i[2]; - Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]); - switch(o){ - case 0: idx[0]=1; idx[1]=2; break; - case 1: idx[0]=0; idx[1]=2; break; - case 2: idx[0]=0; idx[1]=1; break; - }; - return __edgeNeighbor(o,i,idx,forceChildren); - } - template - const OctNode* OctNode::edgeNeighbor(const int& edgeIndex) const { - int idx[2],o,i[2]; - Cube::FactorEdgeIndex(edgeIndex,o,i[0],i[1]); - switch(o){ - case 0: idx[0]=1; idx[1]=2; break; - case 1: idx[0]=0; idx[1]=2; break; - case 2: idx[0]=0; idx[1]=1; break; - }; - return __edgeNeighbor(o,i,idx); - } - template - const OctNode* OctNode::__edgeNeighbor(const int& o,const int i[2],const int idx[2]) const{ - if(!parent){return NULL;} - int pIndex=int(this-parent->children); - int aIndex,x[DIMENSION]; - - Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); - aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3; - pIndex^=(7 ^ (1<__faceNeighbor(idx[0],i[0]); - if(!temp || !temp->children){return NULL;} - else{return &temp->children[pIndex];} - } - else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor - const OctNode* temp=parent->__faceNeighbor(idx[1],i[1]); - if(!temp || !temp->children){return NULL;} - else{return &temp->children[pIndex];} - } - else if(aIndex==0) { // I can get the neighbor from the parent + else if (aIndex == 0) + { // I can get the neighbor from the parent return &parent->children[pIndex]; } - else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor - const OctNode* temp=parent->__edgeNeighbor(o,i,idx); - if(!temp || !temp->children){return temp;} - else{return &temp->children[pIndex];} - } - else{return NULL;} - } - template - OctNode* OctNode::__edgeNeighbor(const int& o,const int i[2],const int idx[2],const int& forceChildren){ - if(!parent){return NULL;} - int pIndex=int(this-parent->children); - int aIndex,x[DIMENSION]; - - Cube::FactorCornerIndex(pIndex,x[0],x[1],x[2]); - aIndex=(~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]])<<1))) & 3; - pIndex^=(7 ^ (1<__faceNeighbor(idx[0],i[0],0); - if(!temp || !temp->children){return NULL;} - else{return &temp->children[pIndex];} - } - else if(aIndex==2) { // I can get the neighbor from the parent's face adjacent neighbor - OctNode* temp=parent->__faceNeighbor(idx[1],i[1],0); - if(!temp || !temp->children){return NULL;} - else{return &temp->children[pIndex];} - } - else if(aIndex==0) { // I can get the neighbor from the parent + else if (aIndex == 3) + { // I can get the neighbor from the parent's edge adjacent neighbor + const OctNode* temp = parent->__edgeNeighbor (o, i, idx); + if (!temp || !temp->children) + { + return temp; + } + else + { + return &temp->children[pIndex]; + } + } + else + { + return NULL; + } + } + + template + OctNode* OctNode + ::__edgeNeighbor (const int& o, const int i[2], const int idx[2], const int& forceChildren) + { + if (!parent) + { + return NULL; + } + int pIndex = int(this-parent->children); + int aIndex, x[DIMENSION]; + + Cube::FactorCornerIndex (pIndex, x[0], x[1], x[2]); + aIndex = (~((i[0] ^ x[idx[0]]) | ((i[1] ^ x[idx[1]]) << 1))) & 3; + pIndex ^= (7 ^ (1 << o)); + if (aIndex == 1) + { // I can get the neighbor from the parent's face adjacent neighbor + OctNode* temp = parent->__faceNeighbor (idx[0], i[0], 0); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return &temp->children[pIndex]; + } + } + else if (aIndex == 2) + { // I can get the neighbor from the parent's face adjacent neighbor + OctNode* temp = parent->__faceNeighbor (idx[1], i[1], 0); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return &temp->children[pIndex]; + } + } + else if (aIndex == 0) + { // I can get the neighbor from the parent return &parent->children[pIndex]; } - else if(aIndex==3) { // I can get the neighbor from the parent's edge adjacent neighbor - OctNode* temp=parent->__edgeNeighbor(o,i,idx,forceChildren); - if(!temp){return NULL;} - if(!temp->children){ - if(forceChildren){temp->initChildren();} - else{return temp;} + else if (aIndex == 3) + { // I can get the neighbor from the parent's edge adjacent neighbor + OctNode* temp = parent->__edgeNeighbor (o, i, idx, forceChildren); + if (!temp) + { + return NULL; + } + if (!temp->children) + { + if (forceChildren) + { + temp->initChildren (); + } + else + { + return temp; + } } return &temp->children[pIndex]; } - else{return NULL;} + else + { + return NULL; + } } - template - const OctNode* OctNode::cornerNeighbor(const int& cornerIndex) const { - int pIndex,aIndex=0; - if(!parent){return NULL;} + template + const OctNode* OctNode + ::cornerNeighbor (const int& cornerIndex) const + { + int pIndex, aIndex = 0; + if (!parent) + { + return NULL; + } - pIndex=int(this-parent->children); - aIndex=(cornerIndex ^ pIndex); // The disagreement bits - pIndex=(~pIndex)&7; // The antipodal point - if(aIndex==7){ // Agree on no bits + pIndex = int(this-parent->children); + aIndex = (cornerIndex ^ pIndex); // The disagreement bits + pIndex = (~pIndex)&7; // The antipodal point + if (aIndex == 7) + { // Agree on no bits return &parent->children[pIndex]; } - else if(aIndex==0){ // Agree on all bits - const OctNode* temp=((const OctNode*)parent)->cornerNeighbor(cornerIndex); - if(!temp || !temp->children){return temp;} - else{return &temp->children[pIndex];} - } - else if(aIndex==6){ // Agree on face 0 - const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} - } - else if(aIndex==5){ // Agree on face 1 - const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} - } - else if(aIndex==3){ // Agree on face 2 - const OctNode* temp=((const OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} - } - else if(aIndex==4){ // Agree on edge 2 - const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) ); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} - } - else if(aIndex==2){ // Agree on edge 1 - const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) ); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} - } - else if(aIndex==1){ // Agree on edge 0 - const OctNode* temp=((const OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 ); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} - } - else{return NULL;} - } - template - OctNode* OctNode::cornerNeighbor(const int& cornerIndex,const int& forceChildren){ - int pIndex,aIndex=0; - if(!parent){return NULL;} - - pIndex=int(this-parent->children); - aIndex=(cornerIndex ^ pIndex); // The disagreement bits - pIndex=(~pIndex)&7; // The antipodal point - if(aIndex==7){ // Agree on no bits + else if (aIndex == 0) + { // Agree on all bits + const OctNode* temp = ((const OctNode*) parent)->cornerNeighbor (cornerIndex); + if (!temp || !temp->children) + { + return temp; + } + else + { + return &temp->children[pIndex]; + } + } + else if (aIndex == 6) + { // Agree on face 0 + const OctNode* temp = ((const OctNode*) parent)->__faceNeighbor (0, cornerIndex & 1); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } + } + else if (aIndex == 5) + { // Agree on face 1 + const OctNode* temp = ((const OctNode*) parent)->__faceNeighbor (1, (cornerIndex & 2) >> 1); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } + } + else if (aIndex == 3) + { // Agree on face 2 + const OctNode* temp = ((const OctNode*) parent)->__faceNeighbor (2, (cornerIndex & 4) >> 2); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } + } + else if (aIndex == 4) + { // Agree on edge 2 + const OctNode* temp = ((const OctNode*) parent)->edgeNeighbor (8 | (cornerIndex & 1) | (cornerIndex & 2)); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } + } + else if (aIndex == 2) + { // Agree on edge 1 + const OctNode* temp = ((const OctNode*) parent)->edgeNeighbor (4 | (cornerIndex & 1) | ((cornerIndex & 4) >> 1)); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } + } + else if (aIndex == 1) + { // Agree on edge 0 + const OctNode* temp = ((const OctNode*) parent)->edgeNeighbor (((cornerIndex & 2) | (cornerIndex & 4)) >> 1); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } + } + else + { + return NULL; + } + } + + template + OctNode* OctNode + ::cornerNeighbor (const int& cornerIndex, const int& forceChildren) + { + int pIndex, aIndex = 0; + if (!parent) + { + return NULL; + } + + pIndex = int(this-parent->children); + aIndex = (cornerIndex ^ pIndex); // The disagreement bits + pIndex = (~pIndex)&7; // The antipodal point + if (aIndex == 7) + { // Agree on no bits return &parent->children[pIndex]; } - else if(aIndex==0){ // Agree on all bits - OctNode* temp=((OctNode*)parent)->cornerNeighbor(cornerIndex,forceChildren); - if(!temp){return NULL;} - if(!temp->children){ - if(forceChildren){temp->initChildren();} - else{return temp;} + else if (aIndex == 0) + { // Agree on all bits + OctNode* temp = ((OctNode*) parent)->cornerNeighbor (cornerIndex, forceChildren); + if (!temp) + { + return NULL; + } + if (!temp->children) + { + if (forceChildren) + { + temp->initChildren (); + } + else + { + return temp; + } } return &temp->children[pIndex]; } - else if(aIndex==6){ // Agree on face 0 - OctNode* temp=((OctNode*)parent)->__faceNeighbor(0,cornerIndex & 1,0); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} + else if (aIndex == 6) + { // Agree on face 0 + OctNode* temp = ((OctNode*) parent)->__faceNeighbor (0, cornerIndex & 1, 0); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } + } + else if (aIndex == 5) + { // Agree on face 1 + OctNode* temp = ((OctNode*) parent)->__faceNeighbor (1, (cornerIndex & 2) >> 1, 0); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } } - else if(aIndex==5){ // Agree on face 1 - OctNode* temp=((OctNode*)parent)->__faceNeighbor(1,(cornerIndex & 2)>>1,0); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} + else if (aIndex == 3) + { // Agree on face 2 + OctNode* temp = ((OctNode*) parent)->__faceNeighbor (2, (cornerIndex & 4) >> 2, 0); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } } - else if(aIndex==3){ // Agree on face 2 - OctNode* temp=((OctNode*)parent)->__faceNeighbor(2,(cornerIndex & 4)>>2,0); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} + else if (aIndex == 4) + { // Agree on edge 2 + OctNode* temp = ((OctNode*) parent)->edgeNeighbor (8 | (cornerIndex & 1) | (cornerIndex & 2)); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } } - else if(aIndex==4){ // Agree on edge 2 - OctNode* temp=((OctNode*)parent)->edgeNeighbor(8 | (cornerIndex & 1) | (cornerIndex & 2) ); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} + else if (aIndex == 2) + { // Agree on edge 1 + OctNode* temp = ((OctNode*) parent)->edgeNeighbor (4 | (cornerIndex & 1) | ((cornerIndex & 4) >> 1)); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } } - else if(aIndex==2){ // Agree on edge 1 - OctNode* temp=((OctNode*)parent)->edgeNeighbor(4 | (cornerIndex & 1) | ((cornerIndex & 4)>>1) ); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} + else if (aIndex == 1) + { // Agree on edge 0 + OctNode* temp = ((OctNode*) parent)->edgeNeighbor (((cornerIndex & 2) | (cornerIndex & 4)) >> 1); + if (!temp || !temp->children) + { + return NULL; + } + else + { + return & temp->children[pIndex]; + } } - else if(aIndex==1){ // Agree on edge 0 - OctNode* temp=((OctNode*)parent)->edgeNeighbor(((cornerIndex & 2) | (cornerIndex & 4))>>1 ); - if(!temp || !temp->children){return NULL;} - else{return & temp->children[pIndex];} + else + { + return NULL; } - else{return NULL;} } //////////////////////// // OctNodeNeighborKey // //////////////////////// - template - OctNode::Neighbors::Neighbors(void){clear();} - template - void OctNode::Neighbors::clear(void){ - for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}} - } - template - OctNode::NeighborKey::NeighborKey(void){neighbors=NULL;} - template - OctNode::NeighborKey::~NeighborKey(void){ - if(neighbors){delete[] neighbors;} - neighbors=NULL; - } - - template - void OctNode::NeighborKey::set(const int& d){ - if(neighbors){delete[] neighbors;} - neighbors=NULL; - if(d<0){return;} - neighbors=new Neighbors[d+1]; - } - template - typename OctNode::Neighbors& OctNode::NeighborKey::setNeighbors(OctNode* node){ - int d=node->depth(); - if(node!=neighbors[d].neighbors[1][1][1]){ - neighbors[d].clear(); - - if(!node->parent){neighbors[d].neighbors[1][1][1]=node;} - else{ - int i,j,k,x1,y1,z1,x2,y2,z2; - int idx=int(node-node->parent->children); - Cube::FactorCornerIndex( idx ,x1,y1,z1); - Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); - for(i=0;i<2;i++){ - for(j=0;j<2;j++){ - for(k=0;k<2;k++){ - neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; + + template + OctNode + ::Neighbors::Neighbors (void) + { + clear (); + } + + template + void OctNode + ::Neighbors::clear (void) + { + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + for (int k = 0; k < 3; k++) + { + neighbors[i][j][k] = NULL; + } + } + } + } + + template + OctNode + ::NeighborKey::NeighborKey (void) + { + neighbors = NULL; + } + + template + OctNode + ::NeighborKey::~NeighborKey (void) + { + if (neighbors) + { + delete[] neighbors; + } + neighbors = NULL; + } + + template + void OctNode + ::NeighborKey::set (const int& d) + { + if (neighbors) + { + delete[] neighbors; + } + neighbors = NULL; + if (d < 0) + { + return; + } + neighbors = new Neighbors[d + 1]; + } + + template + typename OctNode::Neighbors& OctNode + ::NeighborKey::setNeighbors (OctNode* node) + { + int d = node->depth (); + if (node != neighbors[d].neighbors[1][1][1]) + { + neighbors[d].clear (); + + if (!node->parent) + { + neighbors[d].neighbors[1][1][1] = node; + } + else + { + int i, j, k, x1, y1, z1, x2, y2, z2; + int idx = int(node - node->parent->children); + Cube::FactorCornerIndex (idx, x1, y1, z1); + Cube::FactorCornerIndex ((~idx)&7, x2, y2, z2); + for (i = 0; i < 2; i++) + { + for (j = 0; j < 2; j++) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[x2 + i][y2 + j][z2 + k] = &node->parent->children[Cube::CornerIndex (i, j, k)]; } } } - Neighbors& temp=setNeighbors(node->parent); + Neighbors& temp = setNeighbors (node->parent); // Set the neighbors from across the faces - i=x1<<1; - if(temp.neighbors[i][1][1]){ - if(!temp.neighbors[i][1][1]->children){temp.neighbors[i][1][1]->initChildren();} - for(j=0;j<2;j++){for(k=0;k<2;k++){neighbors[d].neighbors[i][y2+j][z2+k]=&temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];}} + i = x1 << 1; + if (temp.neighbors[i][1][1]) + { + if (!temp.neighbors[i][1][1]->children) + { + temp.neighbors[i][1][1]->initChildren (); + } + for (j = 0; j < 2; j++) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[i][y2 + j][z2 + k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex (x2, j, k)]; + } + } } - j=y1<<1; - if(temp.neighbors[1][j][1]){ - if(!temp.neighbors[1][j][1]->children){temp.neighbors[1][j][1]->initChildren();} - for(i=0;i<2;i++){for(k=0;k<2;k++){neighbors[d].neighbors[x2+i][j][z2+k]=&temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)];}} + j = y1 << 1; + if (temp.neighbors[1][j][1]) + { + if (!temp.neighbors[1][j][1]->children) + { + temp.neighbors[1][j][1]->initChildren (); + } + for (i = 0; i < 2; i++) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[x2 + i][j][z2 + k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex (i, y2, k)]; + } + } } - k=z1<<1; - if(temp.neighbors[1][1][k]){ - if(!temp.neighbors[1][1][k]->children){temp.neighbors[1][1][k]->initChildren();} - for(i=0;i<2;i++){for(j=0;j<2;j++){neighbors[d].neighbors[x2+i][y2+j][k]=&temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)];}} + k = z1 << 1; + if (temp.neighbors[1][1][k]) + { + if (!temp.neighbors[1][1][k]->children) + { + temp.neighbors[1][1][k]->initChildren (); + } + for (i = 0; i < 2; i++) + { + for (j = 0; j < 2; j++) + { + neighbors[d].neighbors[x2 + i][y2 + j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex (i, j, z2)]; + } + } } // Set the neighbors from across the edges - i=x1<<1; j=y1<<1; - if(temp.neighbors[i][j][1]){ - if(!temp.neighbors[i][j][1]->children){temp.neighbors[i][j][1]->initChildren();} - for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];} + i = x1 << 1; + j = y1 << 1; + if (temp.neighbors[i][j][1]) + { + if (!temp.neighbors[i][j][1]->children) + { + temp.neighbors[i][j][1]->initChildren (); + } + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[i][j][z2 + k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex (x2, y2, k)]; + } } - i=x1<<1; k=z1<<1; - if(temp.neighbors[i][1][k]){ - if(!temp.neighbors[i][1][k]->children){temp.neighbors[i][1][k]->initChildren();} - for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];} + i = x1 << 1; + k = z1 << 1; + if (temp.neighbors[i][1][k]) + { + if (!temp.neighbors[i][1][k]->children) + { + temp.neighbors[i][1][k]->initChildren (); + } + for (j = 0; j < 2; j++) + { + neighbors[d].neighbors[i][y2 + j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex (x2, j, z2)]; + } } - j=y1<<1; k=z1<<1; - if(temp.neighbors[1][j][k]){ - if(!temp.neighbors[1][j][k]->children){temp.neighbors[1][j][k]->initChildren();} - for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];} + j = y1 << 1; + k = z1 << 1; + if (temp.neighbors[1][j][k]) + { + if (!temp.neighbors[1][j][k]->children) + { + temp.neighbors[1][j][k]->initChildren (); + } + for (i = 0; i < 2; i++) + { + neighbors[d].neighbors[x2 + i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex (i, y2, z2)]; + } } // Set the neighbor from across the corner - i=x1<<1; j=y1<<1; k=z1<<1; - if(temp.neighbors[i][j][k]){ - if(!temp.neighbors[i][j][k]->children){temp.neighbors[i][j][k]->initChildren();} - neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + i = x1 << 1; + j = y1 << 1; + k = z1 << 1; + if (temp.neighbors[i][j][k]) + { + if (!temp.neighbors[i][j][k]->children) + { + temp.neighbors[i][j][k]->initChildren (); + } + neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex (x2, y2, z2)]; } } } return neighbors[d]; } - template - typename OctNode::Neighbors& OctNode::NeighborKey::getNeighbors(OctNode* node){ - int d=node->depth(); - if(node!=neighbors[d].neighbors[1][1][1]){ - neighbors[d].clear(); - - if(!node->parent){neighbors[d].neighbors[1][1][1]=node;} - else{ - int i,j,k,x1,y1,z1,x2,y2,z2; - int idx=int(node-node->parent->children); - Cube::FactorCornerIndex( idx ,x1,y1,z1); - Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); - for(i=0;i<2;i++){ - for(j=0;j<2;j++){ - for(k=0;k<2;k++){ - neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; + template + typename OctNode::Neighbors& OctNode + ::NeighborKey::getNeighbors (OctNode* node) + { + int d = node->depth (); + if (node != neighbors[d].neighbors[1][1][1]) + { + neighbors[d].clear (); + + if (!node->parent) + { + neighbors[d].neighbors[1][1][1] = node; + } + else + { + int i, j, k, x1, y1, z1, x2, y2, z2; + int idx = int(node - node->parent->children); + Cube::FactorCornerIndex (idx, x1, y1, z1); + Cube::FactorCornerIndex ((~idx)&7, x2, y2, z2); + for (i = 0; i < 2; i++) + { + for (j = 0; j < 2; j++) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[x2 + i][y2 + j][z2 + k] = &node->parent->children[Cube::CornerIndex (i, j, k)]; } } } - Neighbors& temp=getNeighbors(node->parent); + Neighbors& temp = getNeighbors (node->parent); // Set the neighbors from across the faces - i=x1<<1; - if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){ - for(j=0;j<2;j++){for(k=0;k<2;k++){neighbors[d].neighbors[i][y2+j][z2+k]=&temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];}} + i = x1 << 1; + if (temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children) + { + for (j = 0; j < 2; j++) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[i][y2 + j][z2 + k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex (x2, j, k)]; + } + } } - j=y1<<1; - if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){ - for(i=0;i<2;i++){for(k=0;k<2;k++){neighbors[d].neighbors[x2+i][j][z2+k]=&temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)];}} + j = y1 << 1; + if (temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children) + { + for (i = 0; i < 2; i++) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[x2 + i][j][z2 + k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex (i, y2, k)]; + } + } } - k=z1<<1; - if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){ - for(i=0;i<2;i++){for(j=0;j<2;j++){neighbors[d].neighbors[x2+i][y2+j][k]=&temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)];}} + k = z1 << 1; + if (temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children) + { + for (i = 0; i < 2; i++) + { + for (j = 0; j < 2; j++) + { + neighbors[d].neighbors[x2 + i][y2 + j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex (i, j, z2)]; + } + } } // Set the neighbors from across the edges - i=x1<<1; j=y1<<1; - if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){ - for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];} + i = x1 << 1; + j = y1 << 1; + if (temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[i][j][z2 + k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex (x2, y2, k)]; + } } - i=x1<<1; k=z1<<1; - if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){ - for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];} + i = x1 << 1; + k = z1 << 1; + if (temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children) + { + for (j = 0; j < 2; j++) + { + neighbors[d].neighbors[i][y2 + j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex (x2, j, z2)]; + } } - j=y1<<1; k=z1<<1; - if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){ - for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];} + j = y1 << 1; + k = z1 << 1; + if (temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children) + { + for (i = 0; i < 2; i++) + { + neighbors[d].neighbors[x2 + i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex (i, y2, z2)]; + } } // Set the neighbor from across the corner - i=x1<<1; j=y1<<1; k=z1<<1; - if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){ - neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + i = x1 << 1; + j = y1 << 1; + k = z1 << 1; + if (temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children) + { + neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex (x2, y2, z2)]; } } } - return neighbors[node->depth()]; + return neighbors[node->depth ()]; } ///////////////////////// // OctNodeNeighborKey2 // ///////////////////////// - template - OctNode::Neighbors2::Neighbors2(void){clear();} - template - void OctNode::Neighbors2::clear(void){ - for(int i=0;i<3;i++){for(int j=0;j<3;j++){for(int k=0;k<3;k++){neighbors[i][j][k]=NULL;}}} - } - template - OctNode::NeighborKey2::NeighborKey2(void){neighbors=NULL;} - template - OctNode::NeighborKey2::~NeighborKey2(void){ - if(neighbors){delete[] neighbors;} - neighbors=NULL; - } - - template - void OctNode::NeighborKey2::set(const int& d){ - if(neighbors){delete[] neighbors;} - neighbors=NULL; - if(d<0){return;} - neighbors=new Neighbors2[d+1]; - } - template - typename OctNode::Neighbors2& OctNode::NeighborKey2::getNeighbors(const OctNode* node){ - int d=node->depth(); - if(node!=neighbors[d].neighbors[1][1][1]){ - neighbors[d].clear(); - - if(!node->parent){neighbors[d].neighbors[1][1][1]=node;} - else{ - int i,j,k,x1,y1,z1,x2,y2,z2; - int idx=int(node-node->parent->children); - Cube::FactorCornerIndex( idx ,x1,y1,z1); - Cube::FactorCornerIndex((~idx)&7,x2,y2,z2); - for(i=0;i<2;i++){ - for(j=0;j<2;j++){ - for(k=0;k<2;k++){ - neighbors[d].neighbors[x2+i][y2+j][z2+k]=&node->parent->children[Cube::CornerIndex(i,j,k)]; + + template + OctNode + ::Neighbors2::Neighbors2 (void) + { + clear (); + } + + template + void OctNode + ::Neighbors2::clear (void) + { + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + for (int k = 0; k < 3; k++) + { + neighbors[i][j][k] = NULL; + } + } + } + } + + template + OctNode + ::NeighborKey2::NeighborKey2 (void) + { + neighbors = NULL; + } + + template + OctNode + ::NeighborKey2::~NeighborKey2 (void) + { + if (neighbors) + { + delete[] neighbors; + } + neighbors = NULL; + } + + template + void OctNode + ::NeighborKey2::set (const int& d) + { + if (neighbors) + { + delete[] neighbors; + } + neighbors = NULL; + if (d < 0) + { + return; + } + neighbors = new Neighbors2[d + 1]; + } + + template + typename OctNode::Neighbors2& OctNode + ::NeighborKey2::getNeighbors (const OctNode* node) + { + int d = node->depth (); + if (node != neighbors[d].neighbors[1][1][1]) + { + neighbors[d].clear (); + + if (!node->parent) + { + neighbors[d].neighbors[1][1][1] = node; + } + else + { + int i, j, k, x1, y1, z1, x2, y2, z2; + int idx = int(node - node->parent->children); + Cube::FactorCornerIndex (idx, x1, y1, z1); + Cube::FactorCornerIndex ((~idx)&7, x2, y2, z2); + for (i = 0; i < 2; i++) + { + for (j = 0; j < 2; j++) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[x2 + i][y2 + j][z2 + k] = &node->parent->children[Cube::CornerIndex (i, j, k)]; } } } - Neighbors2& temp=getNeighbors(node->parent); + Neighbors2& temp = getNeighbors (node->parent); // Set the neighbors from across the faces - i=x1<<1; - if(temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children){ - for(j=0;j<2;j++){for(k=0;k<2;k++){neighbors[d].neighbors[i][y2+j][z2+k]=&temp.neighbors[i][1][1]->children[Cube::CornerIndex(x2,j,k)];}} + i = x1 << 1; + if (temp.neighbors[i][1][1] && temp.neighbors[i][1][1]->children) + { + for (j = 0; j < 2; j++) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[i][y2 + j][z2 + k] = &temp.neighbors[i][1][1]->children[Cube::CornerIndex (x2, j, k)]; + } + } } - j=y1<<1; - if(temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children){ - for(i=0;i<2;i++){for(k=0;k<2;k++){neighbors[d].neighbors[x2+i][j][z2+k]=&temp.neighbors[1][j][1]->children[Cube::CornerIndex(i,y2,k)];}} + j = y1 << 1; + if (temp.neighbors[1][j][1] && temp.neighbors[1][j][1]->children) + { + for (i = 0; i < 2; i++) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[x2 + i][j][z2 + k] = &temp.neighbors[1][j][1]->children[Cube::CornerIndex (i, y2, k)]; + } + } } - k=z1<<1; - if(temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children){ - for(i=0;i<2;i++){for(j=0;j<2;j++){neighbors[d].neighbors[x2+i][y2+j][k]=&temp.neighbors[1][1][k]->children[Cube::CornerIndex(i,j,z2)];}} + k = z1 << 1; + if (temp.neighbors[1][1][k] && temp.neighbors[1][1][k]->children) + { + for (i = 0; i < 2; i++) + { + for (j = 0; j < 2; j++) + { + neighbors[d].neighbors[x2 + i][y2 + j][k] = &temp.neighbors[1][1][k]->children[Cube::CornerIndex (i, j, z2)]; + } + } } // Set the neighbors from across the edges - i=x1<<1; j=y1<<1; - if(temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children){ - for(k=0;k<2;k++){neighbors[d].neighbors[i][j][z2+k]=&temp.neighbors[i][j][1]->children[Cube::CornerIndex(x2,y2,k)];} + i = x1 << 1; + j = y1 << 1; + if (temp.neighbors[i][j][1] && temp.neighbors[i][j][1]->children) + { + for (k = 0; k < 2; k++) + { + neighbors[d].neighbors[i][j][z2 + k] = &temp.neighbors[i][j][1]->children[Cube::CornerIndex (x2, y2, k)]; + } } - i=x1<<1; k=z1<<1; - if(temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children){ - for(j=0;j<2;j++){neighbors[d].neighbors[i][y2+j][k]=&temp.neighbors[i][1][k]->children[Cube::CornerIndex(x2,j,z2)];} + i = x1 << 1; + k = z1 << 1; + if (temp.neighbors[i][1][k] && temp.neighbors[i][1][k]->children) + { + for (j = 0; j < 2; j++) + { + neighbors[d].neighbors[i][y2 + j][k] = &temp.neighbors[i][1][k]->children[Cube::CornerIndex (x2, j, z2)]; + } } - j=y1<<1; k=z1<<1; - if(temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children){ - for(i=0;i<2;i++){neighbors[d].neighbors[x2+i][j][k]=&temp.neighbors[1][j][k]->children[Cube::CornerIndex(i,y2,z2)];} + j = y1 << 1; + k = z1 << 1; + if (temp.neighbors[1][j][k] && temp.neighbors[1][j][k]->children) + { + for (i = 0; i < 2; i++) + { + neighbors[d].neighbors[x2 + i][j][k] = &temp.neighbors[1][j][k]->children[Cube::CornerIndex (i, y2, z2)]; + } } // Set the neighbor from across the corner - i=x1<<1; j=y1<<1; k=z1<<1; - if(temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children){ - neighbors[d].neighbors[i][j][k]=&temp.neighbors[i][j][k]->children[Cube::CornerIndex(x2,y2,z2)]; + i = x1 << 1; + j = y1 << 1; + k = z1 << 1; + if (temp.neighbors[i][j][k] && temp.neighbors[i][j][k]->children) + { + neighbors[d].neighbors[i][j][k] = &temp.neighbors[i][j][k]->children[Cube::CornerIndex (x2, y2, z2)]; } } } - return neighbors[node->depth()]; + return neighbors[node->depth ()]; } - template - int OctNode::write(const char* fileName) const{ - FILE* fp=fopen(fileName,"wb"); - if(!fp){return 0;} - int ret=write(fp); - fclose(fp); + template + int OctNode + ::write (const char* fileName) const + { + FILE* fp = fopen (fileName, "wb"); + if (!fp) + { + return 0; + } + int ret = write (fp); + fclose (fp); return ret; } - template - int OctNode::write(FILE* fp) const{ - fwrite(this,sizeof(OctNode),1,fp); - if(children){for(int i=0;i + int OctNode + ::write (FILE* fp) const + { + fwrite (this, sizeof (OctNode), 1, fp); + if (children) + { + for (int i = 0; i < Cube::CORNERS; i++) + { + children[i].write (fp); + } + } return 1; } - template - int OctNode::read(const char* fileName){ - FILE* fp=fopen(fileName,"rb"); - if(!fp){return 0;} - int ret=read(fp); - fclose(fp); + + template + int OctNode + ::read (const char* fileName) + { + FILE* fp = fopen (fileName, "rb"); + if (!fp) + { + return 0; + } + int ret = read (fp); + fclose (fp); return ret; } - template - int OctNode::read(FILE* fp){ - fread(this,sizeof(OctNode),1,fp); - parent=NULL; - if(children){ - children=NULL; - initChildren(); - for(int i=0;i + int OctNode + ::read (FILE* fp) + { + fread (this, sizeof (OctNode), 1, fp); + parent = NULL; + if (children) + { + children = NULL; + initChildren (); + for (int i = 0; i < Cube::CORNERS; i++) + { + children[i].read (fp); + children[i].parent = this; } } return 1; } - template - int OctNode::width(const int& maxDepth) const { - int d=depth(); - return 1<<(maxDepth-d); + + template + int OctNode + ::width (const int& maxDepth) const + { + int d = depth (); + return 1 << (maxDepth - d); } - template - void OctNode::centerIndex(const int& maxDepth,int index[DIMENSION]) const { - int d,o[3]; - depthAndOffset(d,o); - for(int i=0;i::CornerIndex(maxDepth,d+1,o[i]<<1,1);} + + template + void OctNode + ::centerIndex (const int& maxDepth, int index[DIMENSION]) const + { + int d, o[3]; + depthAndOffset (d, o); + for (int i = 0; i < DIMENSION; i++) + { + index[i] = BinaryNode::CornerIndex (maxDepth, d + 1, o[i] << 1, 1); + } } diff --git a/surface/include/pcl/surface/poisson/allocator.h b/surface/include/pcl/surface/poisson/allocator.h index 3240252d357..11ef231f16c 100644 --- a/surface/include/pcl/surface/poisson/allocator.h +++ b/surface/include/pcl/surface/poisson/allocator.h @@ -160,7 +160,7 @@ namespace pcl { return NULL; } if(remains(memory.size() - 1)){ mem=new T[blockSize]; if(!mem){fprintf(stderr,"Failed to allocate memory\n");exit(0);} memory.push_back(mem); diff --git a/surface/src/poisson.cpp b/surface/src/poisson.cpp index 399a80ddcda..5bb60644080 100644 --- a/surface/src/poisson.cpp +++ b/surface/src/poisson.cpp @@ -37,7 +37,7 @@ #ifdef WIN32 #pragma optimize("g", off) -#endif WIN32 +#endif //WIN32 #include "pcl/impl/instantiate.hpp" #include "pcl/point_types.h" diff --git a/surface/src/poisson/geometry.cpp b/surface/src/poisson/geometry.cpp index 0eb7d029492..e6739463efd 100644 --- a/surface/src/poisson/geometry.cpp +++ b/surface/src/poisson/geometry.cpp @@ -64,7 +64,7 @@ namespace pcl { int CoredVectorMeshData::addPolygon( const std::vector< CoredVertexIndex >& vertices ) { std::vector< int > polygon( vertices.size() ); - for( int i=0 ; i& polygon = polygons[ polygonIndex++ ]; vertices.resize( polygon.size() ); - for( int i=0 ; i < static_cast (polygon.size ()); i++ ) + for( unsigned i=0 ; i