multi_grid_octree_data.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement  (BSD License)
00003  *
00004  *  Point Cloud Library  (PCL) - www.pointclouds.org
00005  *  Copyright  (c) 2009-2012, Willow Garage, Inc.
00006  *  Copyright  (c) 2006, Michael Kazhdan and Matthew Bolitho,
00007  *                      Johns Hopkins University
00008  *
00009  *  All rights reserved.
00010  *
00011  *  Redistribution and use in source and binary forms, with or without
00012  *  modification, are permitted provided that the following conditions
00013  *  are met:
00014  *
00015  *   * Redistributions of source code must retain the above copyright
00016  *     notice, this list of conditions and the following disclaimer.
00017  *   * Redistributions in binary form must reproduce the above
00018  *     copyright notice, this list of conditions and the following
00019  *     disclaimer in the documentation and/or other materials provided
00020  *     with the distribution.
00021  *   * Neither the name of Willow Garage, Inc. nor the names of its
00022  *     contributors may be used to endorse or promote products derived
00023  *     from this software without specific prior written permission.
00024  *
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00027  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00030  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES  (INCLUDING,
00031  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *  LIABILITY, OR TORT  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  *
00038  * $Id: multi_grid_octree_data.hpp 5170 2012-03-18 04:21:56Z rusu $
00039  *
00040  */
00041 #include <pcl/surface/poisson/octree_poisson.h>
00042 
00043 #define ITERATION_POWER 1.0/3
00044 #define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12
00045 
00046 #define READ_SIZE 1024
00047 
00048 #define PAD_SIZE  (Real (1.0))
00049 
00050 namespace pcl
00051 {
00052   namespace poisson
00053   {
00054     const Real EPSILON = Real (1e-6);
00055     const Real ROUND_EPS = Real (1e-5);
00056 
00058     SortedTreeNodes::SortedTreeNodes () : treeNodes (NULL), nodeCount (NULL), maxDepth (0)
00059     {
00060     }
00061 
00063     SortedTreeNodes::~SortedTreeNodes ()
00064     {
00065       if (nodeCount)
00066         delete[] nodeCount;
00067       if (treeNodes)
00068         delete[] treeNodes;
00069       nodeCount = NULL;
00070       treeNodes = NULL;
00071     }
00072 
00074     void
00075     SortedTreeNodes::set (TreeOctNode& root, const int& setIndex)
00076     {
00077       if (nodeCount)
00078         delete[] nodeCount;
00079       if (treeNodes)
00080         delete[] treeNodes;
00081       maxDepth = root.maxDepth () + 1;
00082       nodeCount = new int[maxDepth + 1];
00083       treeNodes = new TreeOctNode*[root.nodes ()];
00084 
00085       TreeOctNode* temp = root.nextNode ();
00086       int i, cnt = 0;
00087       while (temp)
00088       {
00089         treeNodes[cnt++] = temp;
00090         temp = root.nextNode (temp);
00091       }
00092       qsort (treeNodes, cnt, sizeof (const TreeOctNode*), TreeOctNode::CompareForwardPointerDepths);
00093       for (i = 0; i <= maxDepth; i++)
00094         nodeCount[i] = 0;
00095       for (i = 0; i < cnt; i++)
00096       {
00097         if (setIndex)
00098           treeNodes[i]->nodeData.nodeIndex = i;
00099         nodeCount[treeNodes[i]->depth () + 1]++;
00100       }
00101       for (i = 1; i <= maxDepth; i++)
00102         nodeCount[i] += nodeCount[i - 1];
00103     }
00104 
00106     int TreeNodeData::UseIndex = 1;
00107 
00109     TreeNodeData::TreeNodeData () : value (0)
00110     {
00111       if (UseIndex)
00112       {
00113         nodeIndex = -1;
00114         centerWeightContribution = 0;
00115       }
00116       else
00117       {
00118         mcIndex = 0;
00119       }
00120     }
00121 
00123     TreeNodeData::~TreeNodeData ()
00124     {
00125     }
00126 
00128     template<int Degree> double Octree<Degree>::maxMemoryUsage = 0;
00129 
00131     template<int Degree> double 
00132     Octree<Degree>::MemoryUsage ()
00133     {
00134       double mem = 0.0; //MemoryInfo::Usage ()/ (1<<20);
00135       if (mem > maxMemoryUsage)
00136         maxMemoryUsage = mem;
00137       return (mem);
00138     }
00139 
00141     template<int Degree>
00142     Octree<Degree>::Octree () : 
00143       neighborKey (), neighborKey2 (), 
00144       radius (0), width (0), normals (), postNormalSmooth (0), tree (), fData ()
00145     {
00146     }
00147 
00149     template<int Degree> void 
00150     Octree<Degree>::setNodeIndices (TreeOctNode& node, int& idx)
00151     {
00152       node.nodeData.nodeIndex = idx;
00153       idx++;
00154       if (node.children)
00155         for (int i = 0; i < Cube::CORNERS; i++)
00156           setNodeIndices (node.children[i], idx);
00157     }
00158 
00159 
00161     template<int Degree> int 
00162     Octree<Degree>::NonLinearSplatOrientedPoint (TreeOctNode* node, const Point3D<Real>& position, const Point3D<Real>& normal)
00163     {
00164       double x, dxdy, dxdydz, dx[DIMENSION][3];
00165       int i, j, k;
00166       TreeOctNode::Neighbors& neighbors = neighborKey.setNeighbors (node);
00167       double width;
00168       Point3D<Real> center;
00169       Real w;
00170 
00171       node->centerAndWidth (center, w);
00172       width = w;
00173       for (int i = 0; i < 3; i++)
00174       {
00175         x = (center.coords[i] - position.coords[i] - width) / width;
00176         dx[i][0] = 1.125 + 1.500 * x + 0.500 * x*x;
00177         x = (center.coords[i] - position.coords[i]) / width;
00178         dx[i][1] = 0.750 - x*x;
00179         dx[i][2] = 1.0 - dx[i][1] - dx[i][0];
00180       }
00181 
00182       for (i = 0; i < 3; i++)
00183       {
00184         for (j = 0; j < 3; j++)
00185         {
00186           dxdy = dx[0][i] * dx[1][j];
00187           for (k = 0; k < 3; k++)
00188           {
00189             if (neighbors.neighbors[i][j][k])
00190             {
00191               dxdydz = dxdy * dx[2][k];
00192               int idx = neighbors.neighbors[i][j][k]->nodeData.nodeIndex;
00193               if (idx < 0)
00194               {
00195                 Point3D<Real> n;
00196                 n.coords[0] = n.coords[1] = n.coords[2] = 0;
00197                 idx = neighbors.neighbors[i][j][k]->nodeData.nodeIndex = int (normals->size ());
00198                 normals->push_back (n);
00199               }
00200               (*normals)[idx].coords[0] += Real (normal.coords[0] * dxdydz);
00201               (*normals)[idx].coords[1] += Real (normal.coords[1] * dxdydz);
00202               (*normals)[idx].coords[2] += Real (normal.coords[2] * dxdydz);
00203             }
00204           }
00205         }
00206       }
00207       return (0);
00208     }
00209 
00211     template<int Degree> void 
00212     Octree<Degree>::NonLinearSplatOrientedPoint (const Point3D<Real>& position,
00213                                                  const Point3D<Real>& normal,
00214                                                  const int& splatDepth,
00215                                                  const Real& samplesPerNode,
00216                                                  const int& minDepth,
00217                                                  const int& maxDepth)
00218     {
00219       double dx;
00220       Point3D<Real> n;
00221       TreeOctNode* temp;
00222       int i; //,cnt = 0;
00223       double width;
00224       Point3D<Real> myCenter;
00225       Real myWidth;
00226       myCenter.coords[0] = myCenter.coords[1] = myCenter.coords[2] = Real (0.5);
00227       myWidth = Real (1.0);
00228 
00229       temp = &tree;
00230       while (temp->depth () < splatDepth)
00231       {
00232         if (!temp->children)
00233         {
00234           PCL_ERROR ("Octree<Degree>::NonLinearSplatOrientedPoint error\n");
00235           return;
00236         }
00237 
00238         int cIndex = TreeOctNode::CornerIndex (myCenter, position);
00239         temp = &temp->children[cIndex];
00240         myWidth /= 2;
00241         if (cIndex & 1)
00242           myCenter.coords[0] += myWidth / 2;
00243         else
00244           myCenter.coords[0] -= myWidth / 2;
00245 
00246         if (cIndex & 2)
00247           myCenter.coords[1] += myWidth / 2;
00248         else
00249           myCenter.coords[1] -= myWidth / 2;
00250 
00251         if (cIndex & 4)
00252           myCenter.coords[2] += myWidth / 2;
00253         else
00254           myCenter.coords[2] -= myWidth / 2;
00255       }
00256 
00257       Real alpha, newDepth;
00258       NonLinearGetSampleDepthAndWeight (temp, position, samplesPerNode, newDepth, alpha);
00259 
00260       if (newDepth < minDepth)
00261         newDepth = Real (minDepth);
00262       if (newDepth > maxDepth)
00263         newDepth = Real (maxDepth);
00264 
00265       int topDepth = int (ceil (newDepth));
00266 
00267       //dx = static_cast<Real> (static_cast<Real> (1.0 - topDepth) - newDepth);
00268       dx = 1.0 - topDepth + newDepth;
00269       if (topDepth <= minDepth)
00270       {
00271         topDepth = minDepth;
00272         dx = 1;
00273       }
00274       else if (topDepth > maxDepth)
00275       {
00276         topDepth = maxDepth;
00277         dx = 1;
00278       }
00279       while (temp->depth () > topDepth)
00280         temp = temp->parent;
00281       while (temp->depth () < topDepth)
00282       {
00283         if (!temp->children)
00284           temp->initChildren ();
00285         int cIndex = TreeOctNode::CornerIndex (myCenter, position);
00286         temp = &temp->children[cIndex];
00287         myWidth /= 2;
00288         if (cIndex & 1)
00289           myCenter.coords[0] += myWidth / 2;
00290         else
00291           myCenter.coords[0] -= myWidth / 2;
00292         if (cIndex & 2)
00293           myCenter.coords[1] += myWidth / 2;
00294         else
00295           myCenter.coords[1] -= myWidth / 2;
00296         if (cIndex & 4)
00297           myCenter.coords[2] += myWidth / 2;
00298         else
00299           myCenter.coords[2] -= myWidth / 2;
00300       }
00301       width = 1.0 / (1 << temp->depth ());
00302       for (i = 0; i < DIMENSION; i++)
00303         n.coords[i] = normal.coords[i] * alpha / Real (pow (width, 3)) * Real (dx);
00304       NonLinearSplatOrientedPoint (temp, position, n);
00305       if (fabs (1.0 - dx) > EPSILON)
00306       {
00307         dx = Real (1.0 - dx);
00308         temp = temp->parent;
00309         width = 1.0 / (1 << temp->depth ());
00310 
00311         for (i = 0; i < DIMENSION; i++)
00312           n.coords[i] = normal.coords[i] * alpha / Real (pow (width, 3)) * Real (dx);
00313         NonLinearSplatOrientedPoint (temp, position, n);
00314       }
00315     }
00316 
00318     template<int Degree> void 
00319     Octree<Degree>::NonLinearGetSampleDepthAndWeight (TreeOctNode* node,
00320                                                       const Point3D<Real>& position,
00321                                                       const Real& samplesPerNode,
00322                                                       Real& depth,
00323                                                       Real& weight)
00324     {
00325       TreeOctNode* temp = node;
00326       weight = Real (1.0) / NonLinearGetSampleWeight (temp, position);
00327       if (weight >= samplesPerNode + 1)
00328         depth = Real (temp->depth () + log (weight / (samplesPerNode + 1)) / log (double (1 << (DIMENSION - 1))));
00329       else
00330       {
00331         Real oldAlpha, newAlpha;
00332         oldAlpha = newAlpha = weight;
00333         while (newAlpha < (samplesPerNode + 1) && temp->parent)
00334         {
00335           temp = temp->parent;
00336           oldAlpha = newAlpha;
00337           newAlpha = Real (1.0) / NonLinearGetSampleWeight (temp, position);
00338         }
00339         depth = Real (temp->depth () + log (newAlpha / (samplesPerNode + 1)) / log (newAlpha / oldAlpha));
00340       }
00341       weight = Real (pow (double (1 << (DIMENSION - 1)), -double (depth)));
00342     }
00343 
00344 
00346     template<int Degree> Real 
00347     Octree<Degree>::NonLinearGetSampleWeight (TreeOctNode* node, const Point3D<Real>& position)
00348     {
00349       Real weight = 0;
00350       double x, dxdy, dx[DIMENSION][3];
00351       int i, j, k;
00352       TreeOctNode::Neighbors& neighbors = neighborKey.setNeighbors (node);
00353       double width;
00354       Point3D<Real> center;
00355       Real w;
00356       node->centerAndWidth (center, w);
00357       width = w;
00358 
00359       for (i = 0; i < DIMENSION; i++)
00360       {
00361         x = (center.coords[i] - position.coords[i] - width) / width;
00362         dx[i][0] = 1.125 + 1.500 * x + 0.500 * x*x;
00363         x = (center.coords[i] - position.coords[i]) / width;
00364         dx[i][1] = 0.750 - x*x;
00365         dx[i][2] = 1.0 - dx[i][1] - dx[i][0];
00366       }
00367 
00368       for (i = 0; i < 3; i++)
00369       {
00370         for (j = 0; j < 3; j++)
00371         {
00372           dxdy = dx[0][i] * dx[1][j];
00373           for (k = 0; k < 3; k++)
00374           {
00375             if (neighbors.neighbors[i][j][k])
00376               weight += Real (dxdy * dx[2][k] * neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution);
00377           }
00378         }
00379       }
00380       return (Real (1.0 / weight));
00381     }
00382 
00384     template<int Degree> int 
00385     Octree<Degree>::NonLinearUpdateWeightContribution (TreeOctNode* node,
00386                                                        const Point3D<Real>& position,
00387                                                        const Real& weight)
00388     {
00389       int i, j, k;
00390       TreeOctNode::Neighbors& neighbors = neighborKey.setNeighbors (node);
00391       double x, dxdy, dx[DIMENSION][3];
00392       double width;
00393       Point3D<Real> center;
00394       Real w;
00395       node->centerAndWidth (center, w);
00396       width = w;
00397 
00398       for (i = 0; i < DIMENSION; i++)
00399       {
00400         x = (center.coords[i] - position.coords[i] - width) / width;
00401         dx[i][0] = 1.125 + 1.500 * x + 0.500 * x*x;
00402         x = (center.coords[i] - position.coords[i]) / width;
00403         dx[i][1] = 0.750 - x*x;
00404         dx[i][2] = 1.0 - dx[i][1] - dx[i][0];
00405       }
00406 
00407       for (i = 0; i < 3; i++)
00408       {
00409         for (j = 0; j < 3; j++)
00410         {
00411           dxdy = dx[0][i] * dx[1][j] * weight;
00412           for (k = 0; k < 3; k++)
00413             if (neighbors.neighbors[i][j][k])
00414               neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution += Real (dxdy * dx[2][k]);
00415         }
00416       }
00417       return (0);
00418     }
00419 
00421     template<int Degree> template<typename PointNT> int 
00422     Octree<Degree>::setTree (boost::shared_ptr<const pcl::PointCloud<PointNT> > input_,
00423                              const int& maxDepth,
00424                              const int& kernelDepth,
00425                              const Real& samplesPerNode,
00426                              const Real& scaleFactor,
00427                              Point3D<Real>& center,
00428                              Real& scale,
00429                              const int& resetSamples,
00430                              const int& useConfidence)
00431     {
00432       Point3D<Real> min, max, position, normal, myCenter;
00433       Real myWidth;
00434       size_t i, cnt = 0;
00435       TreeOctNode* temp;
00436       int splatDepth = 0;
00437       float c[2 * DIMENSION];
00438 
00439       TreeNodeData::UseIndex = 1;
00440       neighborKey.set (maxDepth);
00441       splatDepth = kernelDepth;
00442       if (splatDepth < 0)
00443         splatDepth = 0;
00444 
00445       // Read through once to get the center and scale
00446       while (1)
00447       {
00448         if (cnt == input_->size ())
00449           break;
00450         c[0] = input_->points[cnt].x;
00451         c[1] = input_->points[cnt].y;
00452         c[2] = input_->points[cnt].z;
00453         c[3] = input_->points[cnt].normal_x;
00454         c[4] = input_->points[cnt].normal_y;
00455         c[5] = input_->points[cnt].normal_z;
00456 
00457         for (i = 0; i < DIMENSION; i++)
00458         {
00459           if (!cnt || c[i] < min.coords[i])
00460             min.coords[i] = c[i];
00461           if (!cnt || c[i] > max.coords[i])
00462             max.coords[i] = c[i];
00463         }
00464         cnt++;
00465       }
00466 
00467       for (i = 0; i < DIMENSION; i++)
00468       {
00469         if (!i || scale < max.coords[i] - min.coords[i])
00470           scale = Real (max.coords[i] - min.coords[i]);
00471         center.coords[i] = Real (max.coords[i] + min.coords[i]) / 2;
00472       }
00473 
00474       scale *= scaleFactor;
00475       for (i = 0; i < DIMENSION; i++)
00476         center.coords[i] -= scale / 2;
00477       if (splatDepth > 0)
00478       {
00479         cnt = 0;
00480         while (1)
00481         {
00482           if (cnt == input_->size ())
00483             break;
00484           c[0] = input_->points[cnt].x;
00485           c[1] = input_->points[cnt].y;
00486           c[2] = input_->points[cnt].z;
00487           c[3] = input_->points[cnt].normal_x;
00488           c[4] = input_->points[cnt].normal_y;
00489           c[5] = input_->points[cnt].normal_z;
00490 
00491           for (i = 0; i < DIMENSION; i++)
00492           {
00493             position.coords[i] = (c[i] - center.coords[i]) / scale;
00494             normal.coords[i] = c[DIMENSION + i];
00495           }
00496           myCenter.coords[0] = myCenter.coords[1] = myCenter.coords[2] = Real (0.5);
00497           myWidth = Real (1.0);
00498           for (i = 0; i < DIMENSION; i++)
00499             if (position.coords[i] < myCenter.coords[i] - myWidth / 2 || position.coords[i] > myCenter.coords[i] + myWidth / 2)
00500               break;
00501           if (i != DIMENSION)
00502             continue;
00503 
00504           temp = &tree;
00505           int d = 0;
00506           Real weight = Real (1.0);
00507           if (useConfidence)
00508             weight = Real (Length (normal));
00509 
00510           while (d < splatDepth)
00511           {
00512             NonLinearUpdateWeightContribution (temp, position, weight);
00513             if (!temp->children)
00514               temp->initChildren ();
00515             int cIndex = TreeOctNode::CornerIndex (myCenter, position);
00516             temp = &temp->children[cIndex];
00517             myWidth /= 2;
00518             if (cIndex & 1)
00519               myCenter.coords[0] += myWidth / 2;
00520             else
00521               myCenter.coords[0] -= myWidth / 2;
00522             if (cIndex & 2)
00523               myCenter.coords[1] += myWidth / 2;
00524             else
00525               myCenter.coords[1] -= myWidth / 2;
00526             if (cIndex & 4)
00527               myCenter.coords[2] += myWidth / 2;
00528             else
00529               myCenter.coords[2] -= myWidth / 2;
00530             d++;
00531           }
00532           NonLinearUpdateWeightContribution (temp, position, weight);
00533           cnt++;
00534         }
00535       }
00536 
00537       normals = new std::vector<Point3D<Real> > ();
00538       cnt = 0;
00539       while (1)
00540       {
00541         if (cnt == input_->size ())
00542           break;
00543         c[0] = input_->points[cnt].x;
00544         c[1] = input_->points[cnt].y;
00545         c[2] = input_->points[cnt].z;
00546         c[3] = input_->points[cnt].normal_x;
00547         c[4] = input_->points[cnt].normal_y;
00548         c[5] = input_->points[cnt].normal_z;
00549         cnt++;
00550 
00551         for (i = 0; i < DIMENSION; i++)
00552         {
00553           position.coords[i] = (c[i] - center.coords[i]) / scale;
00554           normal.coords[i] = c[DIMENSION + i];
00555         }
00556 
00557         myCenter.coords[0] = myCenter.coords[1] = myCenter.coords[2] = Real (0.5);
00558         myWidth = Real (1.0);
00559         for (i = 0; i < DIMENSION; i++)
00560           if (position.coords[i] < myCenter.coords[i] - myWidth / 2 || position.coords[i] > myCenter.coords[i] + myWidth / 2)
00561             break;
00562         if (i != DIMENSION)
00563           continue;
00564         Real l = Real (Length (normal));
00565         if (l != l || l < EPSILON)
00566           continue;
00567         if (!useConfidence)
00568         {
00569           normal.coords[0] /= l;
00570           normal.coords[1] /= l;
00571           normal.coords[2] /= l;
00572         }
00573         l = Real (2 << maxDepth);
00574         normal.coords[0] *= l;
00575         normal.coords[1] *= l;
00576         normal.coords[2] *= l;
00577 
00578         if (resetSamples && samplesPerNode > 0 && splatDepth)
00579           NonLinearSplatOrientedPoint (position, normal, splatDepth, samplesPerNode, 1, maxDepth);
00580         else
00581         {
00582           Real alpha = 1;
00583           temp = &tree;
00584           int d = 0;
00585           if (splatDepth)
00586           {
00587             while (d < splatDepth)
00588             {
00589               int cIndex = TreeOctNode::CornerIndex (myCenter, position);
00590               temp = &temp->children[cIndex];
00591               myWidth /= 2;
00592               if (cIndex & 1)
00593                 myCenter.coords[0] += myWidth / 2;
00594               else
00595                 myCenter.coords[0] -= myWidth / 2;
00596               if (cIndex & 2)
00597                 myCenter.coords[1] += myWidth / 2;
00598               else
00599                 myCenter.coords[1] -= myWidth / 2;
00600               if (cIndex & 4)
00601                 myCenter.coords[2] += myWidth / 2;
00602               else
00603                 myCenter.coords[2] -= myWidth / 2;
00604               d++;
00605             }
00606             alpha = NonLinearGetSampleWeight (temp, position);
00607           }
00608           for (i = 0; i < DIMENSION; i++)
00609             normal.coords[i] *= alpha;
00610           while (d < maxDepth)
00611           {
00612             if (!temp->children)
00613               temp->initChildren ();
00614             int cIndex = TreeOctNode::CornerIndex (myCenter, position);
00615             temp = &temp->children[cIndex];
00616             myWidth /= 2;
00617             if (cIndex & 1)
00618               myCenter.coords[0] += myWidth / 2;
00619             else
00620               myCenter.coords[0] -= myWidth / 2;
00621             if (cIndex & 2)
00622               myCenter.coords[1] += myWidth / 2;
00623             else
00624               myCenter.coords[1] -= myWidth / 2;
00625             if (cIndex & 4)
00626               myCenter.coords[2] += myWidth / 2;
00627             else
00628               myCenter.coords[2] -= myWidth / 2;
00629             d++;
00630           }
00631           NonLinearSplatOrientedPoint (temp, position, normal);
00632         }
00633       }
00634       return (static_cast<int> (cnt));
00635     }
00636 
00638     template<int Degree> void 
00639     Octree<Degree>::setFunctionData (const PPolynomial<Degree>& ReconstructionFunction,
00640                                      const int& maxDepth, const int& normalize,
00641                                      const Real& normalSmooth)
00642     {
00643       radius = Real (fabs (ReconstructionFunction.polys[0].start));
00644       width = int (double (radius + 0.5 - EPSILON)*2);
00645       if (normalSmooth > 0)
00646         postNormalSmooth = normalSmooth;
00647       fData.set (maxDepth, ReconstructionFunction, normalize, 1);
00648     }
00649 
00651     template<int Degree> void 
00652     Octree<Degree>::finalize1 (const int& refineNeighbors)
00653     {
00654       TreeOctNode* temp;
00655 
00656       if (refineNeighbors >= 0)
00657       {
00658         RefineFunction rf;
00659         temp = tree.nextNode ();
00660         while (temp)
00661         {
00662           if (temp->nodeData.nodeIndex >= 0 && Length ((*normals)[temp->nodeData.nodeIndex]) > EPSILON)
00663           {
00664             rf.depth = temp->depth () - refineNeighbors;
00665             TreeOctNode::ProcessMaxDepthNodeAdjacentNodes (fData.depth, temp, 2 * width, &tree, 1, temp->depth () - refineNeighbors, &rf);
00666           }
00667           temp = tree.nextNode (temp);
00668         }
00669       }
00670       else if (refineNeighbors == -1234)
00671       {
00672         temp = tree.nextLeaf ();
00673         while (temp)
00674         {
00675           if (!temp->children && temp->depth () < fData.depth)
00676             temp->initChildren ();
00677           temp = tree.nextLeaf (temp);
00678         }
00679       }
00680     }
00681 
00682 
00684     template<int Degree> void 
00685     Octree<Degree>::finalize2 (const int& refineNeighbors)
00686     {
00687       TreeOctNode* temp;
00688 
00689       if (refineNeighbors >= 0)
00690       {
00691         RefineFunction rf;
00692         temp = tree.nextNode ();
00693         while (temp)
00694         {
00695           if (fabs (temp->nodeData.value) > EPSILON)
00696           {
00697             rf.depth = temp->depth () - refineNeighbors;
00698             TreeOctNode::ProcessMaxDepthNodeAdjacentNodes (fData.depth, temp, 2 * width, &tree, 1, temp->depth () - refineNeighbors, &rf);
00699           }
00700           temp = tree.nextNode (temp);
00701         }
00702       }
00703     }
00704 
00706     template <int Degree> Real 
00707     Octree<Degree>::GetDivergence (const int idx[DIMENSION], const Point3D<Real>& normal) const
00708     {
00709       double dot = fData.dotTable[idx[0]] * fData.dotTable[idx[1]] * fData.dotTable[idx[2]];
00710       return Real (dot * (fData.dDotTable[idx[0]] * normal.coords[0] + fData.dDotTable[idx[1]] * normal.coords[1] + fData.dDotTable[idx[2]] * normal.coords[2]));
00711     }
00712 
00714     template<int Degree> Real 
00715     Octree<Degree>::GetLaplacian (const int idx[DIMENSION]) const
00716     {
00717       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]]));
00718     }
00719 
00721     template<int Degree> Real 
00722     Octree<Degree>::GetDotProduct (const int idx[DIMENSION]) const
00723     {
00724       return Real (fData.dotTable[idx[0]] * fData.dotTable[idx[1]] * fData.dotTable[idx[2]]);
00725     }
00726 
00728     template<int Degree> int 
00729     Octree<Degree>::GetFixedDepthLaplacian (SparseSymmetricMatrix<float>& matrix,
00730                                             const int& depth,
00731                                             const SortedTreeNodes& sNodes)
00732     {
00733       LaplacianMatrixFunction mf;
00734       mf.ot = this;
00735       mf.offset = sNodes.nodeCount[depth];
00736       matrix.Resize (sNodes.nodeCount[depth + 1] - sNodes.nodeCount[depth]);
00737       mf.rowElements = reinterpret_cast<MatrixEntry<float>*> (malloc (sizeof (MatrixEntry<float>) * matrix.rows));
00738       for (int i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth + 1]; i++)
00739       {
00740         mf.elementCount = 0;
00741         mf.d2 = int (sNodes.treeNodes[i]->d);
00742         mf.x2 = int (sNodes.treeNodes[i]->off[0]);
00743         mf.y2 = int (sNodes.treeNodes[i]->off[1]);
00744         mf.z2 = int (sNodes.treeNodes[i]->off[2]);
00745         mf.index[0] = mf.x2;
00746         mf.index[1] = mf.y2;
00747         mf.index[2] = mf.z2;
00748         TreeOctNode::ProcessTerminatingNodeAdjacentNodes (fData.depth, sNodes.treeNodes[i], 2 * width - 1, &tree, 1, &mf);
00749         matrix.SetRowSize (i - sNodes.nodeCount[depth], mf.elementCount);
00750         memcpy (matrix.m_ppElements[i - sNodes.nodeCount[depth]], mf.rowElements, sizeof (MatrixEntry<float>) * mf.elementCount);
00751       }
00752       free (mf.rowElements);
00753       return 1;
00754     }
00755 
00757     template<int Degree> int 
00758     Octree<Degree>::GetRestrictedFixedDepthLaplacian (SparseSymmetricMatrix<float>& matrix,
00759                                                       const int& /*depth*/,
00760                                                       const int* entries,
00761                                                       const int& entryCount,
00762                                                       const TreeOctNode* rNode,
00763                                                       const Real& radius,
00764                                                       const SortedTreeNodes& sNodes)
00765     {
00766       int i;
00767       RestrictedLaplacianMatrixFunction mf;
00768       //Real myRadius = int (2*radius-ROUND_EPS)+ROUND_EPS;
00769       mf.ot = this;
00770       mf.radius = radius;
00771       rNode->depthAndOffset (mf.depth, mf.offset);
00772       matrix.Resize (entryCount);
00773       mf.rowElements = reinterpret_cast<MatrixEntry<float>*> (malloc (sizeof (MatrixEntry<float>) * matrix.rows));
00774 
00775       for (i = 0; i < entryCount; i++)
00776         sNodes.treeNodes[entries[i]]->nodeData.nodeIndex = i;
00777       for (i = 0; i < entryCount; i++)
00778       {
00779         mf.elementCount = 0;
00780         mf.index[0] = int (sNodes.treeNodes[entries[i]]->off[0]);
00781         mf.index[1] = int (sNodes.treeNodes[entries[i]]->off[1]);
00782         mf.index[2] = int (sNodes.treeNodes[entries[i]]->off[2]);
00783         TreeOctNode::ProcessTerminatingNodeAdjacentNodes (fData.depth, sNodes.treeNodes[entries[i]], 2 * width - 1, &tree, 1, &mf);
00784         matrix.SetRowSize (i, mf.elementCount);
00785         memcpy (matrix.m_ppElements[i], mf.rowElements, sizeof (MatrixEntry<float>) * mf.elementCount);
00786       }
00787       for (i = 0; i < entryCount; i++)
00788         sNodes.treeNodes[entries[i]]->nodeData.nodeIndex = entries[i];
00789       free (mf.rowElements);
00790       return 1;
00791     }
00792 
00794     template<int Degree> int 
00795     Octree<Degree>::LaplacianMatrixIteration (const int& subdivideDepth)
00796     {
00797       int i, iter = 0;
00798       SortedTreeNodes sNodes;
00799       //double t;
00800       fData.setDotTables (fData.D2_DOT_FLAG);
00801       sNodes.set (tree, 1);
00802 
00803       SparseMatrix<float>::SetAllocator (MEMORY_ALLOCATOR_BLOCK_SIZE);
00804 
00805       sNodes.treeNodes[0]->nodeData.value = 0;
00806       for (i = 1; i < sNodes.maxDepth; i++)
00807       {
00808         if (subdivideDepth > 0)
00809           iter += SolveFixedDepthMatrix (i, subdivideDepth, sNodes);
00810         else
00811           iter += SolveFixedDepthMatrix (i, sNodes);
00812       }
00813 
00814       SparseMatrix<float>::AllocatorMatrixEntry.reset ();
00815       fData.clearDotTables (fData.DOT_FLAG | fData.D_DOT_FLAG | fData.D2_DOT_FLAG);
00816       return iter;
00817     }
00818 
00820     template<int Degree> int 
00821     Octree<Degree>::SolveFixedDepthMatrix (const int& depth, const SortedTreeNodes& sNodes)
00822     {
00823       int i, iter = 0;
00824       Vector<double> V, Solution;
00825       SparseSymmetricMatrix<Real> matrix;
00826       Real myRadius;
00827       Real dx, dy, dz;
00828       int x1, x2, y1, y2, z1, z2;
00829       Vector<Real> Diagonal;
00830 
00831       V.Resize (sNodes.nodeCount[depth + 1] - sNodes.nodeCount[depth]);
00832       for (i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth + 1]; i++)
00833         V[i - sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.value;
00834       SparseSymmetricMatrix<float>::AllocatorMatrixEntry.rollBack ();
00835       GetFixedDepthLaplacian (matrix, depth, sNodes);
00836       iter += SparseSymmetricMatrix<Real>::Solve (matrix, V, int (pow (matrix.rows, ITERATION_POWER)), Solution, double (EPSILON), 1);
00837 
00838       for (i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth + 1]; i++)
00839         sNodes.treeNodes[i]->nodeData.value = Real (Solution[i - sNodes.nodeCount[depth]]);
00840 
00841       myRadius = Real (radius + ROUND_EPS - 0.5);
00842       myRadius /= static_cast<Real> (1 << depth);
00843 
00844       if (depth < sNodes.maxDepth - 1)
00845       {
00846         LaplacianProjectionFunction pf;
00847         TreeOctNode *node1, *node2;
00848         pf.ot = this;
00849         int idx1, idx2, off = sNodes.nodeCount[depth];
00850         // First pass: idx2 is the solution coefficient propogated
00851         for (i = 0; i < matrix.rows; i++)
00852         {
00853           idx1 = i;
00854           node1 = sNodes.treeNodes[idx1 + off];
00855           if (!node1->children)
00856             continue;
00857           x1 = int (node1->off[0]);
00858           y1 = int (node1->off[1]);
00859           z1 = int (node1->off[2]);
00860           for (int j = 0; j < matrix.rowSizes[i]; j++)
00861           {
00862             idx2 = matrix.m_ppElements[i][j].N;
00863             node2 = sNodes.treeNodes[idx2 + off];
00864             x2 = int (node2->off[0]);
00865             y2 = int (node2->off[1]);
00866             z2 = int (node2->off[2]);
00867             pf.value = Solution[idx2];
00868             pf.index[0] = x2;
00869             pf.index[1] = y2;
00870             pf.index[2] = z2;
00871             dx = Real (x2 - x1) / static_cast<Real> (1 << depth);
00872             dy = Real (y2 - y1) / static_cast<Real> (1 << depth);
00873             dz = Real (z2 - z1) / static_cast<Real> (1 << depth);
00874             if (fabs (dx) < myRadius && fabs (dy) < myRadius && fabs (dz) < myRadius)
00875               node1->processNodeNodes (node2, &pf, 0);
00876             else
00877               TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, node2, width, node1, width, &pf, 0);
00878           }
00879         }
00880         // Second pass: idx1 is the solution coefficient propogated
00881         for (i = 0; i < matrix.rows; i++)
00882         {
00883           idx1 = i;
00884           node1 = sNodes.treeNodes[idx1 + off];
00885           x1 = int (node1->off[0]);
00886           y1 = int (node1->off[1]);
00887           z1 = int (node1->off[2]);
00888           pf.value = Solution[idx1];
00889           pf.index[0] = x1;
00890           pf.index[1] = y1;
00891           pf.index[2] = z1;
00892           for (int j = 0; j < matrix.rowSizes[i]; j++)
00893           {
00894             idx2 = matrix.m_ppElements[i][j].N;
00895             node2 = sNodes.treeNodes[idx2 + off];
00896             if (idx1 != idx2 && node2->children)
00897             {
00898               x2 = int (node2->off[0]);
00899               y2 = int (node2->off[1]);
00900               z2 = int (node2->off[2]);
00901               dx = Real (x1 - x2) / static_cast<Real> (1 << depth);
00902               dy = Real (y1 - y2) / static_cast<Real> (1 << depth);
00903               dz = Real (z1 - z2) / static_cast<Real> (1 << depth);
00904               if (fabs (dx) < myRadius && fabs (dy) < myRadius && fabs (dz) < myRadius)
00905                 node2->processNodeNodes (node1, &pf, 0);
00906               else
00907                 TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, node1, width, node2, width, &pf, 0);
00908             }
00909           }
00910         }
00911       }
00912       return iter;
00913     }
00914 
00915 
00917     template<int Degree> int 
00918     Octree<Degree>::SolveFixedDepthMatrix (const int& depth,
00919                                            const int& startingDepth,
00920                                            const SortedTreeNodes& sNodes)
00921     {
00922       int i, j, d, iter = 0;
00923       SparseSymmetricMatrix<Real> matrix;
00924       AdjacencySetFunction asf;
00925       AdjacencyCountFunction acf;
00926       Vector<Real> Values;
00927       Vector<double> SubValues, SubSolution;
00928       Real myRadius, myRadius2;
00929       Real dx, dy, dz;
00930       Vector<Real> Diagonal;
00931 
00932       if (startingDepth >= depth)
00933         return SolveFixedDepthMatrix (depth, sNodes);
00934 
00935       Values.Resize (sNodes.nodeCount[depth + 1] - sNodes.nodeCount[depth]);
00936 
00937       for (i = sNodes.nodeCount[depth]; i < sNodes.nodeCount[depth + 1]; i++)
00938       {
00939         Values[i - sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.value;
00940         sNodes.treeNodes[i]->nodeData.value = 0;
00941       }
00942 
00943       myRadius = 2 * radius - Real (0.5);
00944       myRadius = static_cast<Real> (int (myRadius - ROUND_EPS)) + ROUND_EPS;
00945       myRadius2 = Real (radius + ROUND_EPS - 0.5);
00946       d = depth - startingDepth;
00947       for (i = sNodes.nodeCount[d]; i < sNodes.nodeCount[d + 1]; i++)
00948       {
00949         TreeOctNode* temp;
00950         // Get all of the entries associated to the subspace
00951         acf.adjacencyCount = 0;
00952         temp = sNodes.treeNodes[i]->nextNode ();
00953         while (temp)
00954         {
00955           if (temp->depth () == depth)
00956           {
00957             acf.Function (temp, temp);
00958             temp = sNodes.treeNodes[i]->nextBranch (temp);
00959           }
00960           else
00961             temp = sNodes.treeNodes[i]->nextNode (temp);
00962         }
00963         for (j = sNodes.nodeCount[d]; j < sNodes.nodeCount[d + 1]; j++)
00964         {
00965           if (i == j)
00966             continue;
00967           TreeOctNode::ProcessFixedDepthNodeAdjacentNodes (fData.depth, sNodes.treeNodes[i], 1, sNodes.treeNodes[j], 2 * width - 1, depth, &acf);
00968         }
00969 
00970         if (!acf.adjacencyCount)
00971           continue;
00972         asf.adjacencies = new int[acf.adjacencyCount];
00973         asf.adjacencyCount = 0;
00974         temp = sNodes.treeNodes[i]->nextNode ();
00975         while (temp
00976                )
00977         {
00978           if (temp->depth () == depth)
00979           {
00980             asf.Function (temp, temp);
00981             temp = sNodes.treeNodes[i]->nextBranch (temp);
00982           }
00983           else
00984             temp = sNodes.treeNodes[i]->nextNode (temp);
00985         }
00986         for (j = sNodes.nodeCount[d]; j < sNodes.nodeCount[d + 1]; j++)
00987         {
00988           if (i == j)
00989             continue;
00990           TreeOctNode::ProcessFixedDepthNodeAdjacentNodes (fData.depth, sNodes.treeNodes[i], 1, sNodes.treeNodes[j], 2 * width - 1, depth, &asf);
00991         }
00992 
00993         // Get the associated vector
00994         SubValues.Resize (asf.adjacencyCount);
00995         for (j = 0; j < asf.adjacencyCount; j++)
00996           SubValues[j] = Values[asf.adjacencies[j] - sNodes.nodeCount[depth]];
00997         SubSolution.Resize (asf.adjacencyCount);
00998         for (j = 0; j < asf.adjacencyCount; j++)
00999           SubSolution[j] = sNodes.treeNodes[asf.adjacencies[j]]->nodeData.value;
01000         // Get the associated matrix
01001         SparseSymmetricMatrix<float>::AllocatorMatrixEntry.rollBack ();
01002         GetRestrictedFixedDepthLaplacian (matrix, depth, asf.adjacencies, asf.adjacencyCount, sNodes.treeNodes[i], myRadius, sNodes);
01003 
01004         // Solve the matrix
01005         iter += SparseSymmetricMatrix<Real>::Solve (matrix, SubValues, int (pow (matrix.rows, ITERATION_POWER)), SubSolution, double (EPSILON), 0);
01006 
01007         LaplacianProjectionFunction lpf;
01008         lpf.ot = this;
01009 
01010         // Update the solution for all nodes in the sub-tree
01011         for (j = 0; j < asf.adjacencyCount; j++)
01012         {
01013           temp = sNodes.treeNodes[asf.adjacencies[j]];
01014           while (temp->depth () > sNodes.treeNodes[i]->depth ())
01015             temp = temp->parent;
01016           if (temp->nodeData.nodeIndex >= sNodes.treeNodes[i]->nodeData.nodeIndex)
01017             sNodes.treeNodes[asf.adjacencies[j]]->nodeData.value = Real (SubSolution[j]);
01018         }
01019         // Update the values in the next depth
01020         int x1, x2, y1, y2, z1, z2;
01021         if (depth < sNodes.maxDepth - 1)
01022         {
01023           int idx1, idx2;
01024           TreeOctNode *node1, *node2;
01025           // First pass: idx2 is the solution coefficient propogated
01026           for (j = 0; j < matrix.rows; j++)
01027           {
01028             idx1 = asf.adjacencies[j];
01029             node1 = sNodes.treeNodes[idx1];
01030             if (!node1->children)
01031               continue;
01032 
01033             x1 = int (node1->off[0]);
01034             y1 = int (node1->off[1]);
01035             z1 = int (node1->off[2]);
01036 
01037             for (int k = 0; k < matrix.rowSizes[j]; k++)
01038             {
01039               idx2 = asf.adjacencies[matrix.m_ppElements[j][k].N];
01040               node2 = sNodes.treeNodes[idx2];
01041               temp = node2;
01042               while (temp->depth () > d)
01043                 temp = temp->parent;
01044               if (temp != sNodes.treeNodes[i])
01045                 continue;
01046               lpf.value = Real (SubSolution[matrix.m_ppElements[j][k].N]);
01047               x2 = int (node2->off[0]);
01048               y2 = int (node2->off[1]);
01049               z2 = int (node2->off[2]);
01050               lpf.index[0] = x2;
01051               lpf.index[1] = y2;
01052               lpf.index[2] = z2;
01053               dx = Real (x2 - x1) / static_cast<Real> (1 << depth);
01054               dy = Real (y2 - y1) / static_cast<Real> (1 << depth);
01055               dz = Real (z2 - z1) / static_cast<Real> (1 << depth);
01056               if (fabs (dx) < myRadius2 && fabs (dy) < myRadius2 && fabs (dz) < myRadius2)
01057                 node1->processNodeNodes (node2, &lpf, 0);
01058               else
01059                 TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, node2, width, node1, width, &lpf, 0);
01060             }
01061           }
01062           // Second pass: idx1 is the solution coefficient propogated
01063           for (j = 0; j < matrix.rows; j++)
01064           {
01065             idx1 = asf.adjacencies[j];
01066             node1 = sNodes.treeNodes[idx1];
01067             temp = node1;
01068             while (temp->depth () > d)
01069               temp = temp->parent;
01070             if (temp != sNodes.treeNodes[i])
01071               continue;
01072             x1 = int (node1->off[0]);
01073             y1 = int (node1->off[1]);
01074             z1 = int (node1->off[2]);
01075 
01076             lpf.value = Real (SubSolution[j]);
01077             lpf.index[0] = x1;
01078             lpf.index[1] = y1;
01079             lpf.index[2] = z1;
01080             for (int k = 0; k < matrix.rowSizes[j]; k++)
01081             {
01082               idx2 = asf.adjacencies[matrix.m_ppElements[j][k].N];
01083               node2 = sNodes.treeNodes[idx2];
01084               if (!node2->children)
01085                 continue;
01086 
01087               if (idx1 != idx2)
01088               {
01089                 x2 = int (node2->off[0]);
01090                 y2 = int (node2->off[1]);
01091                 z2 = int (node2->off[2]);
01092                 dx = Real (x1 - x2) / static_cast<Real> (1 << depth);
01093                 dy = Real (y1 - y2) / static_cast<Real> (1 << depth);
01094                 dz = Real (z1 - z2) / static_cast<Real> (1 << depth);
01095                 if (fabs (dx) < myRadius2 && fabs (dy) < myRadius2 && fabs (dz) < myRadius2)
01096                   node2->processNodeNodes (node1, &lpf, 0);
01097                 else
01098                   TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, node1, width, node2, width, &lpf, 0);
01099               }
01100             }
01101           }
01102         }
01103         delete[] asf.adjacencies;
01104       }
01105       return iter;
01106     }
01107 
01109     template<int Degree> int 
01110     Octree<Degree>::HasNormals (TreeOctNode* node, const Real& epsilon)
01111     {
01112       int hasNormals = 0;
01113       if (node->nodeData.nodeIndex >= 0 && Length ((*normals)[node->nodeData.nodeIndex]) > epsilon)
01114         hasNormals = 1;
01115       if (node->children)
01116         for (int i = 0; i < Cube::CORNERS && !hasNormals; i++)
01117           hasNormals |= HasNormals (&node->children[i], epsilon);
01118 
01119       return hasNormals;
01120     }
01121 
01123     template<int Degree> void 
01124     Octree<Degree>::ClipTree ()
01125     {
01126       TreeOctNode* temp;
01127       temp = tree.nextNode ();
01128       while (temp)
01129       {
01130         if (temp->children)
01131         {
01132           int hasNormals = 0;
01133           for (int i = 0; i < Cube::CORNERS && !hasNormals; i++)
01134             hasNormals = HasNormals (&temp->children[i], EPSILON);
01135           if (!hasNormals)
01136             temp->children = NULL;
01137         }
01138         temp = tree.nextNode (temp);
01139       }
01140     }
01141 
01143     template<int Degree> void 
01144     Octree<Degree>::SetLaplacianWeights ()
01145     {
01146       TreeOctNode* temp;
01147 
01148       fData.setDotTables (fData.DOT_FLAG | fData.D_DOT_FLAG);
01149       DivergenceFunction df;
01150       df.ot = this;
01151       temp = tree.nextNode ();
01152       while (temp)
01153       {
01154         if (temp->nodeData.nodeIndex < 0 || Length ((*normals)[temp->nodeData.nodeIndex]) <= EPSILON)
01155         {
01156           temp = tree.nextNode (temp);
01157           continue;
01158         }
01159         //int d = temp->depth ();
01160         df.normal = (*normals)[temp->nodeData.nodeIndex];
01161         df.index[0] = int (temp->off[0]);
01162         df.index[1] = int (temp->off[1]);
01163         df.index[2] = int (temp->off[2]);
01164         TreeOctNode::ProcessNodeAdjacentNodes (fData.depth, temp, width, &tree, width, &df);
01165         temp = tree.nextNode (temp);
01166       }
01167       fData.clearDotTables (fData.D_DOT_FLAG);
01168       temp = tree.nextNode ();
01169       while (temp)
01170       {
01171         if (temp->nodeData.nodeIndex < 0)
01172           temp->nodeData.centerWeightContribution = 0;
01173         else
01174           temp->nodeData.centerWeightContribution = Real (Length ((*normals)[temp->nodeData.nodeIndex]));
01175         temp = tree.nextNode (temp);
01176       }
01177 
01178       delete normals;
01179       normals = NULL;
01180     }
01181 
01183     template<int Degree> void 
01184     Octree<Degree>::DivergenceFunction::Function (TreeOctNode* node1, const TreeOctNode* /*node2*/)
01185     {
01186       Point3D<Real> n = normal;
01187       if (FunctionData<Degree, Real>::SymmetricIndex (index[0], int (node1->off[0]), scratch[0]))
01188         n.coords[0] = -n.coords[0];
01189       if (FunctionData<Degree, Real>::SymmetricIndex (index[1], int (node1->off[1]), scratch[1]))
01190         n.coords[1] = -n.coords[1];
01191       if (FunctionData<Degree, Real>::SymmetricIndex (index[2], int (node1->off[2]), scratch[2]))
01192         n.coords[2] = -n.coords[2];
01193       double dot = ot->fData.dotTable[scratch[0]] * ot->fData.dotTable[scratch[1]] * ot->fData.dotTable[scratch[2]];
01194       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]));
01195     }
01196 
01198     template<int Degree> void 
01199     Octree<Degree>::LaplacianProjectionFunction::Function (TreeOctNode* node1, const TreeOctNode* /*node2*/)
01200     {
01201       scratch[0] = FunctionData<Degree, Real>::SymmetricIndex (index[0], int (node1->off[0]));
01202       scratch[1] = FunctionData<Degree, Real>::SymmetricIndex (index[1], int (node1->off[1]));
01203       scratch[2] = FunctionData<Degree, Real>::SymmetricIndex (index[2], int (node1->off[2]));
01204       node1->nodeData.value -= Real (ot->GetLaplacian (scratch) * value);
01205     }
01206 
01208     template<int Degree> void 
01209     Octree<Degree>::AdjacencyCountFunction::Function (const TreeOctNode* /*node1*/, const TreeOctNode* /*node2*/)
01210     {
01211       adjacencyCount++;
01212     }
01213 
01215     template<int Degree> void 
01216     Octree<Degree>::AdjacencySetFunction::Function (const TreeOctNode* node1, const TreeOctNode* /*node2*/)
01217     {
01218       adjacencies[adjacencyCount++] = node1->nodeData.nodeIndex;
01219     }
01220 
01222     template<int Degree> void 
01223     Octree<Degree>::RefineFunction::Function (TreeOctNode* node1, const TreeOctNode* /*node2*/)
01224     {
01225       if (!node1->children && node1->depth () < depth)
01226         node1->initChildren ();
01227     }
01228 
01230     template<int Degree> void 
01231     Octree<Degree>::FaceEdgesFunction::Function (const TreeOctNode* node1, const TreeOctNode* /*node2*/)
01232     {
01233       if (!node1->children && MarchingCubes::HasRoots (node1->nodeData.mcIndex))
01234       {
01235         RootInfo ri1, ri2;
01236         hash_map<long long, std::pair<RootInfo, int> >::iterator iter;
01237         int isoTri[DIMENSION * MarchingCubes::MAX_TRIANGLES];
01238         int count = MarchingCubes::AddTriangleIndices (node1->nodeData.mcIndex, isoTri);
01239 
01240         for (int j = 0; j < count; j++)
01241         {
01242           for (int k = 0; k < 3; k++)
01243           {
01244             if (fIndex == Cube::FaceAdjacentToEdges (isoTri[j * 3 + k], isoTri[j * 3 + ((k + 1) % 3)]))
01245             {
01246               if (GetRootIndex (node1, isoTri[j * 3 + k], maxDepth, ri1) && GetRootIndex (node1, isoTri[j * 3 + ((k + 1) % 3)], maxDepth, ri2))
01247               {
01248                 edges->push_back (std::pair<long long, long long> (ri2.key, ri1.key));
01249                 iter = vertexCount->find (ri1.key);
01250                 if (iter == vertexCount->end ())
01251                 {
01252                   (*vertexCount)[ri1.key].first = ri1;
01253                   (*vertexCount)[ri1.key].second = 0;
01254                 }
01255                 iter = vertexCount->find (ri2.key);
01256                 if (iter == vertexCount->end ())
01257                 {
01258                   (*vertexCount)[ri2.key].first = ri2;
01259                   (*vertexCount)[ri2.key].second = 0;
01260                 }
01261                 (*vertexCount)[ri1.key].second--;
01262                 (*vertexCount)[ri2.key].second++;
01263               }
01264               else
01265                 PCL_ERROR ("Bad Edge 1: %d %d\n", ri1.key, ri2.key);
01266             }
01267           }
01268         }
01269       }
01270     }
01271 
01273     template<int Degree> void 
01274     Octree<Degree>::PointIndexValueFunction::Function (const TreeOctNode* node)
01275     {
01276       int idx[DIMENSION];
01277       idx[0] = index[0] + int (node->off[0]);
01278       idx[1] = index[1] + int (node->off[1]);
01279       idx[2] = index[2] + int (node->off[2]);
01280       value += node->nodeData.value * Real (valueTables[idx[0]] * valueTables[idx[1]] * valueTables[idx[2]]);
01281     }
01282 
01284     template<int Degree> void 
01285     Octree<Degree>::PointIndexValueAndNormalFunction::Function (const TreeOctNode* node)
01286     {
01287       int idx[DIMENSION];
01288       idx[0] = index[0] + int (node->off[0]);
01289       idx[1] = index[1] + int (node->off[1]);
01290       idx[2] = index[2] + int (node->off[2]);
01291       value += node->nodeData.value * Real (valueTables[idx[0]] * valueTables[idx[1]] * valueTables[idx[2]]);
01292       normal.coords[0] += node->nodeData.value * Real (dValueTables[idx[0]] * valueTables[idx[1]] * valueTables[idx[2]]);
01293       normal.coords[1] += node->nodeData.value * Real (valueTables[idx[0]] * dValueTables[idx[1]] * valueTables[idx[2]]);
01294       normal.coords[2] += node->nodeData.value * Real (valueTables[idx[0]] * valueTables[idx[1]] * dValueTables[idx[2]]);
01295     }
01296 
01298     template<int Degree> int 
01299     Octree<Degree>::LaplacianMatrixFunction::Function (const TreeOctNode* node1, const TreeOctNode* node2)
01300     {
01301       Real temp;
01302       int d1 = int (node1->d);
01303       int x1, y1, z1;
01304       x1 = int (node1->off[0]);
01305       y1 = int (node1->off[1]);
01306       z1 = int (node1->off[2]);
01307       int dDepth = d2 - d1;
01308       int d;
01309       d = (x2 >> dDepth) - x1;
01310       if (d < 0)
01311         return 0;
01312       if (!dDepth)
01313       {
01314         if (!d)
01315         {
01316           d = y2 - y1;
01317           if (d < 0)
01318             return 0;
01319           else if (!d)
01320           {
01321             d = z2 - z1;
01322             if (d < 0)
01323               return 0;
01324           }
01325         }
01326         scratch[0] = FunctionData<Degree, Real>::SymmetricIndex (index[0], x1);
01327         scratch[1] = FunctionData<Degree, Real>::SymmetricIndex (index[1], y1);
01328         scratch[2] = FunctionData<Degree, Real>::SymmetricIndex (index[2], z1);
01329         temp = ot->GetLaplacian (scratch);
01330         if (node1 == node2)
01331           temp /= 2;
01332         if (fabs (temp) > EPSILON)
01333         {
01334           rowElements[elementCount].Value = temp;
01335           rowElements[elementCount].N = node1->nodeData.nodeIndex - offset;
01336           elementCount++;
01337         }
01338         return 0;
01339       }
01340       return 1;
01341     }
01342 
01344     template<int Degree> int 
01345     Octree<Degree>::RestrictedLaplacianMatrixFunction::Function (const TreeOctNode* node1, const TreeOctNode* node2)
01346     {
01347       int d1, d2, off1[3], off2[3];
01348       node1->depthAndOffset (d1, off1);
01349       node2->depthAndOffset (d2, off2);
01350       int dDepth = d2 - d1;
01351       int d;
01352       d = (off2[0] >> dDepth) - off1[0];
01353       if (d < 0)
01354         return 0;
01355 
01356       if (!dDepth)
01357       {
01358         if (!d)
01359         {
01360           d = off2[1] - off1[1];
01361           if (d < 0)
01362             return 0;
01363           else if (!d)
01364           {
01365             d = off2[2] - off1[2];
01366             if (d < 0)
01367               return 0;
01368           }
01369         }
01370         // Since we are getting the restricted matrix, we don't want to propogate out to terms that don't contribute...
01371         if (!TreeOctNode::Overlap2 (depth, offset, 0.5, d1, off1, radius))
01372           return 0;
01373         scratch[0] = FunctionData<Degree, Real>::SymmetricIndex (index[0], BinaryNode<Real>::Index (d1, off1[0]));
01374         scratch[1] = FunctionData<Degree, Real>::SymmetricIndex (index[1], BinaryNode<Real>::Index (d1, off1[1]));
01375         scratch[2] = FunctionData<Degree, Real>::SymmetricIndex (index[2], BinaryNode<Real>::Index (d1, off1[2]));
01376         Real temp = ot->GetLaplacian (scratch);
01377         if (node1 == node2)
01378           temp /= 2;
01379         if (fabs (temp) > EPSILON)
01380         {
01381           rowElements[elementCount].Value = temp;
01382           rowElements[elementCount].N = node1->nodeData.nodeIndex;
01383           elementCount++;
01384         }
01385         return 0;
01386       }
01387       return 1;
01388     }
01389 
01390 
01392     template<int Degree> void 
01393     Octree<Degree>::GetMCIsoTriangles (const Real& isoValue,
01394                                        CoredMeshData* mesh,
01395                                        const int& fullDepthIso,
01396                                        const int& nonLinearFit,
01397                                        bool addBarycenter,
01398                                        bool polygonMesh)
01399     {
01400       //double t;
01401       TreeOctNode* temp;
01402 
01403       hash_map<long long, int> roots;
01404       hash_map<long long, std::pair<Real, Point3D<Real> > > *normalHash = new hash_map<long long, std::pair<Real, Point3D<Real> > > ();
01405 
01406       SetIsoSurfaceCorners (isoValue, 0, fullDepthIso);
01407       // At the point all of the corner values have been set and all nodes are valid. Now it's just a matter
01408       // of running marching cubes.
01409 
01410       fData.setValueTables (fData.VALUE_FLAG | fData.D_VALUE_FLAG, 0, postNormalSmooth);
01411       temp = tree.nextLeaf ();
01412       while (temp)
01413       {
01414         SetMCRootPositions (temp, 0, isoValue, roots, NULL, *normalHash, NULL, NULL, mesh, nonLinearFit);
01415         temp = tree.nextLeaf (temp);
01416       }
01417 
01418       fData.clearValueTables ();
01419       delete normalHash;
01420 
01421       // Now get the iso-surfaces, running from finest nodes to coarsest in order to allow for edge propogation from
01422       // finer faces to coarser ones.
01423       temp = tree.nextLeaf ();
01424       while (temp)
01425       {
01426         GetMCIsoTriangles (temp, mesh, roots, NULL, NULL, 0, 0, addBarycenter, polygonMesh);
01427         temp = tree.nextLeaf (temp);
01428       }
01429     }
01430 
01431 
01433     template<int Degree> void 
01434     Octree<Degree>::GetMCIsoTriangles (const Real& isoValue,
01435                                        const int& subdivideDepth,
01436                                        CoredMeshData* mesh,
01437                                        const int& fullDepthIso,
01438                                        const int& nonLinearFit,
01439                                        bool addBarycenter,
01440                                        bool polygonMesh)
01441     {
01442       TreeOctNode* temp;
01443       hash_map<long long, int> boundaryRoots, *interiorRoots;
01444       hash_map<long long, std::pair<Real, Point3D<Real> > > *boundaryNormalHash, *interiorNormalHash;
01445       std::vector<Point3D<float> >* interiorPoints;
01446 
01447       int sDepth;
01448       if (subdivideDepth <= 0)
01449         sDepth = 0;
01450       else
01451         sDepth = fData.depth - subdivideDepth;
01452       if (sDepth < 0)
01453         sDepth = 0;
01454 
01455       SetIsoSurfaceCorners (isoValue, sDepth, fullDepthIso);
01456       // At this point all of the corner values have been set and all nodes are valid. Now it's just a matter
01457       // of running marching cubes.
01458 
01459       boundaryNormalHash = new hash_map<long long, std::pair<Real, Point3D<Real> > > ();
01460       int offSet = 0;
01461       SortedTreeNodes sNodes;
01462       sNodes.set (tree, 0);
01463       fData.setValueTables (fData.VALUE_FLAG | fData.D_VALUE_FLAG, 0, postNormalSmooth);
01464 
01465       // Set the root positions for all leaf nodes below the subdivide threshold
01466       SetBoundaryMCRootPositions (sDepth, isoValue, boundaryRoots, *boundaryNormalHash, mesh, nonLinearFit);
01467 
01468       for (int i = sNodes.nodeCount[sDepth]; i < sNodes.nodeCount[sDepth + 1]; i++)
01469       {
01470         interiorRoots = new hash_map<long long, int> ();
01471         interiorNormalHash = new hash_map<long long, std::pair<Real, Point3D<Real> > > ();
01472         interiorPoints = new std::vector<Point3D<float> > ();
01473 
01474         temp = sNodes.treeNodes[i]->nextLeaf ();
01475         while (temp)
01476         {
01477           if (MarchingCubes::HasRoots (temp->nodeData.mcIndex))
01478             SetMCRootPositions (temp, sDepth, isoValue, boundaryRoots, interiorRoots, *boundaryNormalHash, interiorNormalHash, interiorPoints, mesh, nonLinearFit);
01479           temp = sNodes.treeNodes[i]->nextLeaf (temp);
01480         }
01481         delete interiorNormalHash;
01482 
01483         temp = sNodes.treeNodes[i]->nextLeaf ();
01484         while (temp)
01485         {
01486           GetMCIsoTriangles (temp, mesh, boundaryRoots, interiorRoots, interiorPoints, offSet, sDepth, addBarycenter, polygonMesh);
01487           temp = sNodes.treeNodes[i]->nextLeaf (temp);
01488         }
01489         delete interiorRoots;
01490         delete interiorPoints;
01491         offSet = mesh->outOfCorePointCount ();
01492       }
01493       delete boundaryNormalHash;
01494 
01495       temp = tree.nextLeaf ();
01496       while (temp)
01497       {
01498         if (temp->depth () < sDepth)
01499           GetMCIsoTriangles (temp, mesh, boundaryRoots, NULL, NULL, 0, 0, addBarycenter, polygonMesh);
01500         temp = tree.nextLeaf (temp);
01501       }
01502     }
01503 
01504 
01506     template<int Degree> Real 
01507     Octree<Degree>::getCenterValue (const TreeOctNode* node)
01508     {
01509       int idx[3];
01510       Real value = 0;
01511 
01512       neighborKey2.getNeighbors (node);
01513       VertexData::CenterIndex (node, fData.depth, idx);
01514       idx[0] *= fData.res;
01515       idx[1] *= fData.res;
01516       idx[2] *= fData.res;
01517       for (int i = 0; i <= node->depth (); i++)
01518       {
01519         for (int j = 0; j < 3; j++)
01520         {
01521           for (int k = 0; k < 3; k++)
01522           {
01523             for (int l = 0; l < 3; l++)
01524             {
01525               const TreeOctNode* n = neighborKey2.neighbors[i].neighbors[j][k][l];
01526               if (n)
01527               {
01528                 Real temp = n->nodeData.value;
01529                 value += temp * Real (
01530                                       fData.valueTables[idx[0] + int (n->off[0])] *
01531                                       fData.valueTables[idx[1] + int (n->off[1])] *
01532                                       fData.valueTables[idx[2] + int (n->off[2])]);
01533               }
01534             }
01535           }
01536         }
01537       }
01538       if (node->children)
01539       {
01540         for (int i = 0; i < Cube::CORNERS; i++)
01541         {
01542           int ii = Cube::AntipodalCornerIndex (i);
01543           const TreeOctNode* n = &node->children[i];
01544           while (1)
01545           {
01546             value += n->nodeData.value * Real (
01547                                                fData.valueTables[idx[0] + int (n->off[0])] *
01548                                                fData.valueTables[idx[1] + int (n->off[1])] *
01549                                                fData.valueTables[idx[2] + int (n->off[2])]);
01550             if (n->children)
01551             {
01552               n = &n->children[ii];
01553             }
01554             else
01555             {
01556               break;
01557             }
01558           }
01559         }
01560       }
01561       return (static_cast<Real> (value));
01562     }
01563 
01565     template<int Degree> Real 
01566     Octree<Degree>::getCornerValue (const TreeOctNode* node, const int& corner)
01567     {
01568       int idx[3];
01569       Real value = 0;
01570 
01571       neighborKey2.getNeighbors (node);
01572       VertexData::CornerIndex (node, corner, fData.depth, idx);
01573       idx[0] *= fData.res;
01574       idx[1] *= fData.res;
01575       idx[2] *= fData.res;
01576       for (int i = 0; i <= node->depth (); i++)
01577       {
01578         for (int j = 0; j < 3; j++)
01579         {
01580           for (int k = 0; k < 3; k++)
01581           {
01582             for (int l = 0; l < 3; l++)
01583             {
01584               const TreeOctNode* n = neighborKey2.neighbors[i].neighbors[j][k][l];
01585               if (n)
01586               {
01587                 Real temp = n->nodeData.value;
01588                 value += temp * Real (
01589                                       fData.valueTables[idx[0] + int (n->off[0])] *
01590                                       fData.valueTables[idx[1] + int (n->off[1])] *
01591                                       fData.valueTables[idx[2] + int (n->off[2])]);
01592               }
01593             }
01594           }
01595         }
01596       }
01597       int x, y, z, d = node->depth ();
01598       Cube::FactorCornerIndex (corner, x, y, z);
01599       for (int i = 0; i < 2; i++)
01600       {
01601         for (int j = 0; j < 2; j++)
01602         {
01603           for (int k = 0; k < 2; k++)
01604           {
01605             const TreeOctNode* n = neighborKey2.neighbors[d].neighbors[x + i][y + j][z + k];
01606             if (n)
01607             {
01608               int ii = Cube::AntipodalCornerIndex (Cube::CornerIndex (i, j, k));
01609               while (n->children)
01610               {
01611                 n = &n->children[ii];
01612                 value += n->nodeData.value * Real (
01613                                                    fData.valueTables[idx[0] + int (n->off[0])] *
01614                                                    fData.valueTables[idx[1] + int (n->off[1])] *
01615                                                    fData.valueTables[idx[2] + int (n->off[2])]);
01616               }
01617             }
01618           }
01619         }
01620       }
01621       return value;
01622     }
01623 
01625     template<int Degree> void 
01626     Octree<Degree>::getCornerValueAndNormal (const TreeOctNode* node, const int& corner, Real& value, Point3D<Real>& normal)
01627     {
01628       int idx[3], index[3];
01629       value = normal.coords[0] = normal.coords[1] = normal.coords[2] = 0;
01630 
01631       neighborKey2.getNeighbors (node);
01632       VertexData::CornerIndex (node, corner, fData.depth, idx);
01633       idx[0] *= fData.res;
01634       idx[1] *= fData.res;
01635       idx[2] *= fData.res;
01636       for (int i = 0; i <= node->depth (); i++)
01637       {
01638         for (int j = 0; j < 3; j++)
01639         {
01640           for (int k = 0; k < 3; k++)
01641           {
01642             for (int l = 0; l < 3; l++)
01643             {
01644               const TreeOctNode* n = neighborKey2.neighbors[i].neighbors[j][k][l];
01645               if (n)
01646               {
01647                 Real temp = n->nodeData.value;
01648                 index[0] = idx[0] + int (n->off[0]);
01649                 index[1] = idx[1] + int (n->off[1]);
01650                 index[2] = idx[2] + int (n->off[2]);
01651                 value += temp * Real (fData.valueTables[index[0]] * fData.valueTables[index[1]] * fData.valueTables[index[2]]);
01652                 normal.coords[0] += temp * Real (fData.dValueTables[index[0]] * fData.valueTables[index[1]] * fData.valueTables[index[2]]);
01653                 normal.coords[1] += temp * Real (fData.valueTables[index[0]] * fData.dValueTables[index[1]] * fData.valueTables[index[2]]);
01654                 normal.coords[2] += temp * Real (fData.valueTables[index[0]] * fData.valueTables[index[1]] * fData.dValueTables[index[2]]);
01655               }
01656             }
01657           }
01658         }
01659       }
01660       int x, y, z, d = node->depth ();
01661       Cube::FactorCornerIndex (corner, x, y, z);
01662       for (int i = 0; i < 2; i++)
01663       {
01664         for (int j = 0; j < 2; j++)
01665         {
01666           for (int k = 0; k < 2; k++)
01667           {
01668             const TreeOctNode* n = neighborKey2.neighbors[d].neighbors[x + i][y + j][z + k];
01669             if (n)
01670             {
01671               int ii = Cube::AntipodalCornerIndex (Cube::CornerIndex (i, j, k));
01672               while (n->children)
01673               {
01674                 n = &n->children[ii];
01675                 Real temp = n->nodeData.value;
01676                 index[0] = idx[0] + int (n->off[0]);
01677                 index[1] = idx[1] + int (n->off[1]);
01678                 index[2] = idx[2] + int (n->off[2]);
01679                 value += temp * Real (fData.valueTables[index[0]] * fData.valueTables[index[1]] * fData.valueTables[index[2]]);
01680                 normal.coords[0] += temp * Real (fData.dValueTables[index[0]] * fData.valueTables[index[1]] * fData.valueTables[index[2]]);
01681                 normal.coords[1] += temp * Real (fData.valueTables[index[0]] * fData.dValueTables[index[1]] * fData.valueTables[index[2]]);
01682                 normal.coords[2] += temp * Real (fData.valueTables[index[0]] * fData.valueTables[index[1]] * fData.dValueTables[index[2]]);
01683               }
01684             }
01685           }
01686         }
01687       }
01688     }
01689 
01691     template<int Degree> Real 
01692     Octree<Degree>::GetIsoValue ()
01693     {
01694       if (this->width <= 3)
01695       {
01696         TreeOctNode* temp;
01697         Real isoValue, weightSum, w;
01698 
01699         neighborKey2.set (fData.depth);
01700         fData.setValueTables (fData.VALUE_FLAG, 0);
01701 
01702         isoValue = weightSum = 0;
01703         temp = tree.nextNode ();
01704         while (temp)
01705         {
01706           w = temp->nodeData.centerWeightContribution;
01707           if (w > EPSILON)
01708           {
01709             isoValue += getCenterValue (temp) * w;
01710             weightSum += w;
01711           }
01712           temp = tree.nextNode (temp);
01713         }
01714         return isoValue / weightSum;
01715       }
01716       else
01717       {
01718         const TreeOctNode* temp;
01719         Real isoValue, weightSum, w;
01720         //Real myRadius;
01721         PointIndexValueFunction cf;
01722 
01723         fData.setValueTables (fData.VALUE_FLAG, 0);
01724         cf.valueTables = fData.valueTables;
01725         cf.res2 = fData.res2;
01726         //myRadius = radius;
01727         isoValue = weightSum = 0;
01728         temp = tree.nextNode ();
01729         while (temp)
01730         {
01731           w = temp->nodeData.centerWeightContribution;
01732           if (w > EPSILON)
01733           {
01734             cf.value = 0;
01735             int idx[3];
01736             VertexData::CenterIndex (temp, fData.depth, idx);
01737             cf.index[0] = idx[0] * fData.res;
01738             cf.index[1] = idx[1] * fData.res;
01739             cf.index[2] = idx[2] * fData.res;
01740             TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf);
01741             isoValue += cf.value*w;
01742             weightSum += w;
01743           }
01744           temp = tree.nextNode (temp);
01745         }
01746         return isoValue / weightSum;
01747       }
01748     }
01749 
01751     template<int Degree> void 
01752     Octree<Degree>::SetIsoSurfaceCorners (const Real& isoValue, const int& subdivideDepth, const int& /*fullDepthIso*/)
01753     {
01754       int i, j;
01755       hash_map<long long, Real> values;
01756       Real cornerValues[Cube::CORNERS];
01757       PointIndexValueFunction cf;
01758       TreeOctNode* temp;
01759       //int leafCount = tree.leaves ();
01760       long long key;
01761       SortedTreeNodes *sNodes = new SortedTreeNodes ();
01762       sNodes->set (tree, 0);
01763       temp = tree.nextNode ();
01764       while (temp)
01765       {
01766         temp->nodeData.mcIndex = 0;
01767         temp = tree.nextNode (temp);
01768       }
01769       TreeNodeData::UseIndex = 0;
01770       // Start by setting the corner values of all the nodes
01771       cf.valueTables = fData.valueTables;
01772       cf.res2 = fData.res2;
01773       for (i = 0; i < sNodes->nodeCount[subdivideDepth]; i++)
01774       {
01775         temp = sNodes->treeNodes[i];
01776         if (!temp->children)
01777         {
01778           for (j = 0; j < Cube::CORNERS; j++)
01779           {
01780             if (this->width <= 3)
01781             {
01782               cornerValues[j] = getCornerValue (temp, j);
01783             }
01784             else
01785             {
01786               cf.value = 0;
01787               int idx[3];
01788               VertexData::CornerIndex (temp, j, fData.depth, idx);
01789               cf.index[0] = idx[0] * fData.res;
01790               cf.index[1] = idx[1] * fData.res;
01791               cf.index[2] = idx[2] * fData.res;
01792               TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf);
01793               cornerValues[j] = cf.value;
01794             }
01795           }
01796           temp->nodeData.mcIndex = MarchingCubes::GetIndex (cornerValues, isoValue);
01797 
01798           if (temp->parent)
01799           {
01800             TreeOctNode* parent = temp->parent;
01801             int c = int (temp - temp->parent->children);
01802             int mcid = temp->nodeData.mcIndex & (1 << MarchingCubes::cornerMap[c]);
01803 
01804             if (mcid)
01805             {
01806               parent->nodeData.mcIndex |= mcid;
01807               while (1)
01808               {
01809                 if (parent->parent && (parent - parent->parent->children) == c)
01810                 {
01811                   parent->parent->nodeData.mcIndex |= mcid;
01812                   parent = parent->parent;
01813                 }
01814                 else
01815                 {
01816                   break;
01817                 }
01818               }
01819             }
01820           }
01821         }
01822       }
01823 
01824       MemoryUsage ();
01825 
01826       for (i = sNodes->nodeCount[subdivideDepth]; i < sNodes->nodeCount[subdivideDepth + 1]; i++)
01827       {
01828         temp = sNodes->treeNodes[i]->nextLeaf ();
01829         while (temp)
01830         {
01831           for (j = 0; j < Cube::CORNERS; j++)
01832           {
01833             int idx[3];
01834             key = VertexData::CornerIndex (temp, j, fData.depth, idx);
01835             cf.index[0] = idx[0] * fData.res;
01836             cf.index[1] = idx[1] * fData.res;
01837             cf.index[2] = idx[2] * fData.res;
01838             if (values.find (key) != values.end ())
01839             {
01840               cornerValues[j] = values[key];
01841             }
01842             else
01843             {
01844               if (this->width <= 3)
01845               {
01846                 values[key] = cornerValues[j] = getCornerValue (temp, j);
01847               }
01848               else
01849               {
01850                 cf.value = 0;
01851                 TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf);
01852                 values[key] = cf.value;
01853                 cornerValues[j] = cf.value;
01854               }
01855             }
01856           }
01857           temp->nodeData.mcIndex = MarchingCubes::GetIndex (cornerValues, isoValue);
01858 
01859           if (temp->parent)
01860           {
01861             TreeOctNode* parent = temp->parent;
01862             int c = int (temp - temp->parent->children);
01863             int mcid = temp->nodeData.mcIndex & (1 << MarchingCubes::cornerMap[c]);
01864 
01865             if (mcid)
01866             {
01867               parent->nodeData.mcIndex |= mcid;
01868               while (1)
01869               {
01870                 if (parent->parent && (parent - parent->parent->children) == c)
01871                 {
01872                   parent->parent->nodeData.mcIndex |= mcid;
01873                   parent = parent->parent;
01874                 }
01875                 else
01876                 {
01877                   break;
01878                 }
01879               }
01880             }
01881           }
01882 
01883           temp = sNodes->treeNodes[i]->nextLeaf (temp);
01884         }
01885         MemoryUsage ();
01886         values.clear ();
01887       }
01888       delete sNodes;
01889       printf ("Memory Usage: %.3f MB\n", float (MemoryUsage ()));
01890 
01891       if (subdivideDepth)
01892       {
01893         PreValidate (isoValue, fData.depth, subdivideDepth);
01894       }
01895     }
01896 
01898     template<int Degree> void 
01899     Octree<Degree>::Subdivide (TreeOctNode* node, const Real& isoValue, const int& maxDepth)
01900     {
01901       int i, j, c[4];
01902       Real value;
01903       int cornerIndex2[Cube::CORNERS];
01904       PointIndexValueFunction cf;
01905       cf.valueTables = fData.valueTables;
01906       cf.res2 = fData.res2;
01907       node->initChildren ();
01908       // Since we are allocating blocks, it is possible that some of the memory was pre-allocated with
01909       // the wrong initialization
01910 
01911       // Now set the corner values for the new children
01912       // Copy old corner values
01913       for (i = 0; i < Cube::CORNERS; i++)
01914       {
01915         cornerIndex2[i] = node->nodeData.mcIndex & (1 << MarchingCubes::cornerMap[i]);
01916       }
01917       // 8 of 27 corners set
01918 
01919       // Set center corner
01920       cf.value = 0;
01921       int idx[3];
01922       VertexData::CenterIndex (node, maxDepth, idx);
01923       cf.index[0] = idx[0] * fData.res;
01924       cf.index[1] = idx[1] * fData.res;
01925       cf.index[2] = idx[2] * fData.res;
01926       if (this->width <= 3)
01927       {
01928         value = getCenterValue (node);
01929       }
01930       else
01931       {
01932         TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf);
01933         value = cf.value;
01934       }
01935       if (value < isoValue)
01936       {
01937         for (i = 0; i < Cube::CORNERS; i++)
01938         {
01939           cornerIndex2[i] |= 1 << MarchingCubes::cornerMap[Cube::AntipodalCornerIndex (i)];
01940         }
01941       }
01942       // 9 of 27 set
01943 
01944       // Set face corners
01945       for (i = 0; i < Cube::NEIGHBORS; i++)
01946       {
01947         int dir, offset, e;
01948         Cube::FactorFaceIndex (i, dir, offset);
01949         cf.value = 0;
01950         int idx[3];
01951         VertexData::FaceIndex (node, i, maxDepth, idx);
01952         cf.index[0] = idx[0] * fData.res;
01953         cf.index[1] = idx[1] * fData.res;
01954         cf.index[2] = idx[2] * fData.res;
01955         TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf);
01956         value = cf.value;
01957         Cube::FaceCorners (i, c[0], c[1], c[2], c[3]);
01958         e = Cube::EdgeIndex (dir, 0, 0);
01959         if (value < isoValue)
01960         {
01961           for (j = 0; j < 4; j++)
01962           {
01963             cornerIndex2[c[j]] |= 1 << MarchingCubes::cornerMap[Cube::EdgeReflectCornerIndex (c[j], e)];
01964           }
01965         }
01966       }
01967       // 15 of 27 set
01968 
01969       // Set edge corners
01970       for (i = 0; i < Cube::EDGES; i++)
01971       {
01972         int o, i1, i2, f;
01973         Cube::FactorEdgeIndex (i, o, i1, i2);
01974         cf.value = 0;
01975         int idx[3];
01976         VertexData::EdgeIndex (node, i, maxDepth, idx);
01977         cf.index[0] = idx[0] * fData.res;
01978         cf.index[1] = idx[1] * fData.res;
01979         cf.index[2] = idx[2] * fData.res;
01980         TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, width, &cf);
01981         value = cf.value;
01982         Cube::EdgeCorners (i, c[0], c[1]);
01983         f = Cube::FaceIndex (o, 0);
01984         if (value < isoValue)
01985         {
01986           for (j = 0; j < 2; j++)
01987           {
01988             cornerIndex2[c[j]] |= 1 << MarchingCubes::cornerMap[Cube::FaceReflectCornerIndex (c[j], f)];
01989           }
01990         }
01991       }
01992       // 27 of 27 set
01993 
01994       for (i = 0; i < Cube::CORNERS; i++)
01995       {
01996         node->children[i].nodeData.mcIndex = cornerIndex2[i];
01997       }
01998     }
01999 
02000     template<int Degree>
02001     int Octree<Degree>
02002     ::InteriorFaceRootCount (const TreeOctNode* node, const int &faceIndex, const int& maxDepth)
02003     {
02004       int c1, c2, e1, e2, dir, off, cnt = 0;
02005       int corners[Cube::CORNERS / 2];
02006       if (node->children)
02007       {
02008         Cube::FaceCorners (faceIndex, corners[0], corners[1], corners[2], corners[3]);
02009         Cube::FactorFaceIndex (faceIndex, dir, off);
02010         c1 = corners[0];
02011         c2 = corners[3];
02012         switch (dir)
02013         {
02014           case 0:
02015             e1 = Cube::EdgeIndex (1, off, 1);
02016             e2 = Cube::EdgeIndex (2, off, 1);
02017             break;
02018           case 1:
02019             e1 = Cube::EdgeIndex (0, off, 1);
02020             e2 = Cube::EdgeIndex (2, 1, off);
02021             break;
02022           case 2:
02023             e1 = Cube::EdgeIndex (0, 1, off);
02024             e2 = Cube::EdgeIndex (1, 1, off);
02025             break;
02026         };
02027         cnt += EdgeRootCount (&node->children[c1], e1, maxDepth) + EdgeRootCount (&node->children[c1], e2, maxDepth);
02028         switch (dir)
02029         {
02030           case 0:
02031             e1 = Cube::EdgeIndex (1, off, 0);
02032             e2 = Cube::EdgeIndex (2, off, 0);
02033             break;
02034           case 1:
02035             e1 = Cube::EdgeIndex (0, off, 0);
02036             e2 = Cube::EdgeIndex (2, 0, off);
02037             break;
02038           case 2:
02039             e1 = Cube::EdgeIndex (0, 0, off);
02040             e2 = Cube::EdgeIndex (1, 0, off);
02041             break;
02042         };
02043         cnt += EdgeRootCount (&node->children[c2], e1, maxDepth) + EdgeRootCount (&node->children[c2], e2, maxDepth);
02044         for (int i = 0; i < Cube::CORNERS / 2; i++)
02045         {
02046           if (node->children[corners[i]].children)
02047           {
02048             cnt += InteriorFaceRootCount (&node->children[corners[i]], faceIndex, maxDepth);
02049           }
02050         }
02051       }
02052       return cnt;
02053     }
02054 
02056     template<int Degree> int 
02057     Octree<Degree>::EdgeRootCount (const TreeOctNode* node, const int& edgeIndex, const int& maxDepth)
02058     {
02059       int f1, f2, c1, c2;
02060       const TreeOctNode* temp;
02061       Cube::FacesAdjacentToEdge (edgeIndex, f1, f2);
02062 
02063       int eIndex;
02064       const TreeOctNode* finest = node;
02065       eIndex = edgeIndex;
02066       if (node->depth () < maxDepth)
02067       {
02068         temp = node->faceNeighbor (f1);
02069         if (temp && temp->children)
02070         {
02071           finest = temp;
02072           eIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f1);
02073         }
02074         else
02075         {
02076           temp = node->faceNeighbor (f2);
02077           if (temp && temp->children)
02078           {
02079             finest = temp;
02080             eIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f2);
02081           }
02082           else
02083           {
02084             temp = node->edgeNeighbor (edgeIndex);
02085             if (temp && temp->children)
02086             {
02087               finest = temp;
02088               eIndex = Cube::EdgeReflectEdgeIndex (edgeIndex);
02089             }
02090           }
02091         }
02092       }
02093 
02094       Cube::EdgeCorners (eIndex, c1, c2);
02095       if (finest->children)
02096       {
02097         return EdgeRootCount (&finest->children[c1], eIndex, maxDepth) + EdgeRootCount (&finest->children[c2], eIndex, maxDepth);
02098       }
02099       else
02100       {
02101         return MarchingCubes::HasEdgeRoots (finest->nodeData.mcIndex, eIndex);
02102       }
02103     }
02104 
02106     template<int Degree> int 
02107     Octree<Degree>::IsBoundaryFace (const TreeOctNode* node, const int& faceIndex, const int& subdivideDepth)
02108     {
02109       int dir, offset, d, o[3], idx;
02110 
02111       if (subdivideDepth < 0)
02112       {
02113         return 0;
02114       }
02115       if (node->d <= subdivideDepth)
02116       {
02117         return 1;
02118       }
02119       Cube::FactorFaceIndex (faceIndex, dir, offset);
02120       node->depthAndOffset (d, o);
02121 
02122       idx = (int (o[dir]) << 1) + (offset << 1);
02123       return !(idx % (2 << (int (node->d) - subdivideDepth)));
02124     }
02125 
02127     template<int Degree> int 
02128     Octree<Degree>::IsBoundaryEdge (const TreeOctNode* node, const int& edgeIndex, const int& subdivideDepth)
02129     {
02130       int dir, x, y;
02131       Cube::FactorEdgeIndex (edgeIndex, dir, x, y);
02132       return IsBoundaryEdge (node, dir, x, y, subdivideDepth);
02133     }
02134 
02136     template<int Degree> int 
02137     Octree<Degree>::IsBoundaryEdge (const TreeOctNode* node, const int& dir, const int& x, const int& y, const int& subdivideDepth)
02138     {
02139       int d, o[3], idx1, idx2, mask;
02140 
02141       if (subdivideDepth < 0)
02142       {
02143         return 0;
02144       }
02145       if (node->d <= subdivideDepth)
02146       {
02147         return 1;
02148       }
02149       node->depthAndOffset (d, o);
02150 
02151       // initialize to remove warnings
02152       idx1 = idx2 = 0;
02153       switch (dir)
02154       {
02155         case 0:
02156           idx1 = (int (o[1]) << 1) + (x << 1);
02157           idx2 = (int (o[2]) << 1) + (y << 1);
02158           break;
02159         case 1:
02160           idx1 = (int (o[0]) << 1) + (x << 1);
02161           idx2 = (int (o[2]) << 1) + (y << 1);
02162           break;
02163         case 2:
02164           idx1 = (int (o[0]) << 1) + (x << 1);
02165           idx2 = (int (o[1]) << 1) + (y << 1);
02166           break;
02167       }
02168       mask = 2 << (int (node->d) - subdivideDepth);
02169       return !(idx1 % (mask)) || !(idx2 % (mask));
02170     }
02171 
02173     template<int Degree> void 
02174     Octree<Degree>::PreValidate (TreeOctNode* node, const Real& isoValue, const int& maxDepth, const int& subdivideDepth)
02175     {
02176       int sub = 0;
02177       if (node->children)
02178       {
02179         printf ("Bad Pre-Validate\n");
02180       }
02181       //        if (int (node->d)<subdivideDepth){sub = 1;}
02182       for (int i = 0; i < Cube::NEIGHBORS && !sub; i++)
02183       {
02184         TreeOctNode* neighbor = node->faceNeighbor (i);
02185         if (neighbor && neighbor->children)
02186         {
02187           if (IsBoundaryFace (node, i, subdivideDepth) && InteriorFaceRootCount (neighbor, Cube::FaceReflectFaceIndex (i, i), maxDepth))
02188           {
02189             sub = 1;
02190           }
02191         }
02192       }
02193       if (sub)
02194       {
02195         Subdivide (node, isoValue, maxDepth);
02196         for (int i = 0; i < Cube::NEIGHBORS; i++)
02197         {
02198           if (IsBoundaryFace (node, i, subdivideDepth) && InteriorFaceRootCount (node, i, maxDepth))
02199           {
02200             TreeOctNode* neighbor = node->faceNeighbor (i);
02201             while (neighbor && !neighbor->children)
02202             {
02203               PreValidate (neighbor, isoValue, maxDepth, subdivideDepth);
02204               neighbor = node->faceNeighbor (i);
02205             }
02206           }
02207         }
02208       }
02209     }
02210 
02212     template<int Degree> void 
02213     Octree<Degree>::PreValidate (const Real& isoValue, const int& maxDepth, const int& subdivideDepth)
02214     {
02215       TreeOctNode* temp;
02216 
02217       temp = tree.nextLeaf ();
02218       while (temp)
02219       {
02220         PreValidate (temp, isoValue, maxDepth, subdivideDepth);
02221         temp = tree.nextLeaf (temp);
02222       }
02223     }
02224 
02226     template<int Degree> void 
02227     Octree<Degree>::Validate (TreeOctNode* node, const Real& isoValue, const int& maxDepth, const int& fullDepthIso)
02228     {
02229       int i, sub = 0;
02230       TreeOctNode* treeNode = node;
02231       TreeOctNode* neighbor;
02232       if (node->depth () >= maxDepth || node->children)
02233       {
02234         return;
02235       }
02236 
02237       // Check if full-depth extraction is enabled and we have an iso-node that is not at maximum depth
02238       if (!sub && fullDepthIso && MarchingCubes::HasRoots (node->nodeData.mcIndex))
02239       {
02240         sub = 1;
02241       }
02242 
02243       // Check if the node has faces that are ambiguous and are adjacent to finer neighbors
02244       for (i = 0; i < Cube::NEIGHBORS && !sub; i++)
02245       {
02246         neighbor = treeNode->faceNeighbor (i);
02247         if (neighbor && neighbor->children)
02248         {
02249           if (MarchingCubes::IsAmbiguous (node->nodeData.mcIndex, i))
02250           {
02251             sub = 1;
02252           }
02253         }
02254       }
02255 
02256       // Check if the node has edges with more than one root
02257       for (i = 0; i < Cube::EDGES && !sub; i++)
02258       {
02259         if (EdgeRootCount (node, i, maxDepth) > 1)
02260         {
02261           sub = 1;
02262         }
02263       }
02264 
02265       for (i = 0; i < Cube::NEIGHBORS && !sub; i++)
02266       {
02267         neighbor = node->faceNeighbor (i);
02268         if (neighbor && neighbor->children &&
02269             !MarchingCubes::HasFaceRoots (node->nodeData.mcIndex, i) &&
02270             InteriorFaceRootCount (neighbor, Cube::FaceReflectFaceIndex (i, i), maxDepth))
02271         {
02272           sub = 1;
02273         }
02274       }
02275       if (sub)
02276       {
02277         Subdivide (node, isoValue, maxDepth);
02278         for (i = 0; i < Cube::NEIGHBORS; i++)
02279         {
02280           neighbor = treeNode->faceNeighbor (i);
02281           if (neighbor && !neighbor->children)
02282           {
02283             Validate (neighbor, isoValue, maxDepth, fullDepthIso);
02284           }
02285         }
02286         for (i = 0; i < Cube::EDGES; i++)
02287         {
02288           neighbor = treeNode->edgeNeighbor (i);
02289           if (neighbor && !neighbor->children)
02290           {
02291             Validate (neighbor, isoValue, maxDepth, fullDepthIso);
02292           }
02293         }
02294         for (i = 0; i < Cube::CORNERS; i++)
02295         {
02296           if (!node->children[i].children)
02297           {
02298             Validate (&node->children[i], isoValue, maxDepth, fullDepthIso);
02299           }
02300         }
02301       }
02302     }
02303 
02305     template<int Degree> void 
02306     Octree<Degree>::Validate (TreeOctNode* node, const Real& isoValue, const int& maxDepth, const int& fullDepthIso, const int& subdivideDepth)
02307     {
02308       int i, sub = 0;
02309       TreeOctNode* treeNode = node;
02310       TreeOctNode* neighbor;
02311       if (node->depth () >= maxDepth || node->children)
02312       {
02313         return;
02314       }
02315 
02316       // Check if full-depth extraction is enabled and we have an iso-node that is not at maximum depth
02317       if (!sub && fullDepthIso && MarchingCubes::HasRoots (node->nodeData.mcIndex))
02318       {
02319         sub = 1;
02320       }
02321 
02322       // Check if the node has faces that are ambiguous and are adjacent to finer neighbors
02323       for (i = 0; i < Cube::NEIGHBORS && !sub; i++)
02324       {
02325         neighbor = treeNode->faceNeighbor (i);
02326         if (neighbor && neighbor->children)
02327         {
02328           if (MarchingCubes::IsAmbiguous (node->nodeData.mcIndex, i) || IsBoundaryFace (node, i, subdivideDepth))
02329           {
02330             sub = 1;
02331           }
02332         }
02333       }
02334 
02335       // Check if the node has edges with more than one root
02336       for (i = 0; i < Cube::EDGES && !sub; i++)
02337       {
02338         if (EdgeRootCount (node, i, maxDepth) > 1)
02339         {
02340           sub = 1;
02341         }
02342       }
02343 
02344       for (i = 0; i < Cube::NEIGHBORS && !sub; i++)
02345       {
02346         neighbor = node->faceNeighbor (i);
02347         if (neighbor && neighbor->children && !MarchingCubes::HasFaceRoots (node->nodeData.mcIndex, i) &&
02348             InteriorFaceRootCount (neighbor, Cube::FaceReflectFaceIndex (i, i), maxDepth))
02349         {
02350           sub = 1;
02351         }
02352       }
02353       if (sub)
02354       {
02355         Subdivide (node, isoValue, maxDepth);
02356         for (i = 0; i < Cube::NEIGHBORS; i++)
02357         {
02358           neighbor = treeNode->faceNeighbor (i);
02359           if (neighbor && !neighbor->children)
02360           {
02361             Validate (neighbor, isoValue, maxDepth, fullDepthIso, subdivideDepth);
02362           }
02363         }
02364         for (i = 0; i < Cube::EDGES; i++)
02365         {
02366           neighbor = treeNode->edgeNeighbor (i);
02367           if (neighbor && !neighbor->children)
02368           {
02369             Validate (neighbor, isoValue, maxDepth, fullDepthIso, subdivideDepth);
02370           }
02371         }
02372         for (i = 0; i < Cube::CORNERS; i++)
02373         {
02374           if (!node->children[i].children)
02375           {
02376             Validate (&node->children[i], isoValue, maxDepth, fullDepthIso, subdivideDepth);
02377           }
02378         }
02379       }
02380     }
02382     // The assumption made when calling this code is that the edge has at most one root //
02384 
02386     template<int Degree> int 
02387     Octree<Degree>::GetRoot (const RootInfo& ri, const Real& isoValue, Point3D<Real> & position, hash_map<long long, std::pair<Real, Point3D<Real> > >& normalHash, const int& nonLinearFit)
02388     {
02389       int c1, c2;
02390       Cube::EdgeCorners (ri.edgeIndex, c1, c2);
02391       if (!MarchingCubes::HasEdgeRoots (ri.node->nodeData.mcIndex, ri.edgeIndex))
02392       {
02393         return 0;
02394       }
02395 
02396       long long key;
02397       Point3D<Real> n[2];
02398       PointIndexValueAndNormalFunction cnf;
02399       cnf.valueTables = fData.valueTables;
02400       cnf.dValueTables = fData.dValueTables;
02401       cnf.res2 = fData.res2;
02402 
02403       int i, o, i1, i2, rCount = 0;
02404       Polynomial<2> P;
02405       std::vector<double> roots;
02406       double x0, x1;
02407       Real center, width;
02408       Real averageRoot = 0;
02409       Cube::FactorEdgeIndex (ri.edgeIndex, o, i1, i2);
02410       int idx[3];
02411       key = VertexData::CornerIndex (ri.node, c1, fData.depth, idx);
02412       cnf.index[0] = idx[0] * fData.res;
02413       cnf.index[1] = idx[1] * fData.res;
02414       cnf.index[2] = idx[2] * fData.res;
02415 
02416       if (normalHash.find (key) == normalHash.end ())
02417       {
02418         cnf.value = 0;
02419         cnf.normal.coords[0] = cnf.normal.coords[1] = cnf.normal.coords[2] = 0;
02420         // Careful here as the normal isn't quite accurate...  (i.e. postNormalSmooth is ignored)
02421 #if 0
02422         if (this->width <= 3)
02423         {
02424           getCornerValueAndNormal (ri.node, c1, cnf.value, cnf.normal);
02425         }
02426         else
02427         {
02428           TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, this->width, &cnf);
02429         }
02430 #else
02431         TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, this->width, &cnf);
02432 #endif
02433         normalHash[key] = std::pair<Real, Point3D<Real> > (cnf.value, cnf.normal);
02434       }
02435       x0 = normalHash[key].first;
02436       n[0] = normalHash[key].second;
02437 
02438       key = VertexData::CornerIndex (ri.node, c2, fData.depth, idx);
02439       cnf.index[0] = idx[0] * fData.res;
02440       cnf.index[1] = idx[1] * fData.res;
02441       cnf.index[2] = idx[2] * fData.res;
02442       if (normalHash.find (key) == normalHash.end ())
02443       {
02444         cnf.value = 0;
02445         cnf.normal.coords[0] = cnf.normal.coords[1] = cnf.normal.coords[2] = 0;
02446 #if 0
02447         if (this->width <= 3)
02448         {
02449           getCornerValueAndNormal (ri.node, c2, cnf.value, cnf.normal);
02450         }
02451         else
02452         {
02453           TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, this->width, &cnf);
02454         }
02455 #else
02456         TreeOctNode::ProcessPointAdjacentNodes (fData.depth, idx, &tree, this->width, &cnf);
02457 #endif
02458         normalHash[key] = std::pair<Real, Point3D<Real> > (cnf.value, cnf.normal);
02459       }
02460       x1 = normalHash[key].first;
02461       n[1] = normalHash[key].second;
02462 
02463       Point3D<Real> c;
02464       ri.node->centerAndWidth (c, width);
02465       center = c.coords[o];
02466       for (i = 0; i < DIMENSION; i++)
02467       {
02468         n[0].coords[i] *= width;
02469         n[1].coords[i] *= width;
02470       }
02471 
02472       switch (o)
02473       {
02474         case 0:
02475           position.coords[1] = c.coords[1] - width / 2 + width * static_cast<Real> (i1);
02476           position.coords[2] = c.coords[2] - width / 2 + width * static_cast<Real> (i2);
02477           break;
02478         case 1:
02479           position.coords[0] = c.coords[0] - width / 2 + width * static_cast<Real> (i1);
02480           position.coords[2] = c.coords[2] - width / 2 + width * static_cast<Real> (i2);
02481           break;
02482         case 2:
02483           position.coords[0] = c.coords[0] - width / 2 + width * static_cast<Real> (i1);
02484           position.coords[1] = c.coords[1] - width / 2 + width * static_cast<Real> (i2);
02485           break;
02486       }
02487       double dx0, dx1;
02488       dx0 = n[0].coords[o];
02489       dx1 = n[1].coords[o];
02490 
02491       // The scaling will turn the Hermite Spline into a quadratic
02492       double scl = (x1 - x0) / ((dx1 + dx0) / 2);
02493       dx0 *= scl;
02494       dx1 *= scl;
02495 
02496       // Hermite Spline
02497       P.coefficients[0] = x0;
02498       P.coefficients[1] = dx0;
02499       P.coefficients[2] = 3 * (x1 - x0) - dx1 - 2 * dx0;
02500 
02501       P.getSolutions (isoValue, roots, EPSILON);
02502       for (i = 0; i<int (roots.size ()); i++)
02503       {
02504         if (roots[i] >= 0 && roots[i] <= 1)
02505         {
02506           averageRoot += Real (roots[i]);
02507           rCount++;
02508         }
02509       }
02510       if (rCount && nonLinearFit)
02511       {
02512         averageRoot /= static_cast<Real> (rCount);
02513       }
02514       else
02515       {
02516         averageRoot = Real ((x0 - isoValue) / (x0 - x1));
02517       }
02518 
02519       position.coords[o] = Real (center - width / 2 + width * averageRoot);
02520       return 1;
02521     }
02522 
02524     template<int Degree> int 
02525     Octree<Degree>::GetRoot (
02526         const RootInfo& ri, const Real& isoValue, const int& /*maxDepth*/, 
02527         Point3D<Real>& position, hash_map<long long, 
02528         std::pair<Real, Point3D<Real> > >& normals, Point3D<Real>* /*normal*/, const int& nonLinearFit)
02529     {
02530       if (!MarchingCubes::HasRoots (ri.node->nodeData.mcIndex))
02531       {
02532         return 0;
02533       }
02534       return GetRoot (ri, isoValue, position, normals, nonLinearFit);
02535     }
02536 
02538     template<int Degree> int 
02539     Octree<Degree>::GetRootIndex (const TreeOctNode* node, const int& edgeIndex, const int& maxDepth, const int& sDepth, RootInfo& ri)
02540     {
02541       int c1, c2, f1, f2;
02542       const TreeOctNode *temp, *finest;
02543       int finestIndex;
02544 
02545       Cube::FacesAdjacentToEdge (edgeIndex, f1, f2);
02546 
02547       finest = node;
02548       finestIndex = edgeIndex;
02549       if (node->depth () < maxDepth)
02550       {
02551         if (IsBoundaryFace (node, f1, sDepth))
02552         {
02553           temp = NULL;
02554         }
02555         else
02556         {
02557           temp = node->faceNeighbor (f1);
02558         }
02559         if (temp && temp->children)
02560         {
02561           finest = temp;
02562           finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f1);
02563         }
02564         else
02565         {
02566           if (IsBoundaryFace (node, f2, sDepth))
02567           {
02568             temp = NULL;
02569           }
02570           else
02571           {
02572             temp = node->faceNeighbor (f2);
02573           }
02574           if (temp && temp->children)
02575           {
02576             finest = temp;
02577             finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f2);
02578           }
02579           else
02580           {
02581             if (IsBoundaryEdge (node, edgeIndex, sDepth))
02582             {
02583               temp = NULL;
02584             }
02585             else
02586             {
02587               temp = node->edgeNeighbor (edgeIndex);
02588             }
02589             if (temp && temp->children)
02590             {
02591               finest = temp;
02592               finestIndex = Cube::EdgeReflectEdgeIndex (edgeIndex);
02593             }
02594           }
02595         }
02596       }
02597 
02598       Cube::EdgeCorners (finestIndex, c1, c2);
02599       if (finest->children)
02600       {
02601         if (GetRootIndex (&finest->children[c1], finestIndex, maxDepth, sDepth, ri))
02602         {
02603           return 1;
02604         }
02605         else if (GetRootIndex (&finest->children[c2], finestIndex, maxDepth, sDepth, ri))
02606         {
02607           return 1;
02608         }
02609         else
02610         {
02611           return 0;
02612         }
02613       }
02614       else
02615       {
02616         if (!(MarchingCubes::edgeMask[finest->nodeData.mcIndex] & (1 << finestIndex)))
02617         {
02618           return 0;
02619         }
02620 
02621         int o, i1, i2;
02622         Cube::FactorEdgeIndex (finestIndex, o, i1, i2);
02623         int d, off[3];
02624         finest->depthAndOffset (d, off);
02625         ri.node = finest;
02626         ri.edgeIndex = finestIndex;
02627         int eIndex[2], offset;
02628         offset = BinaryNode<Real>::Index (d, off[o]);
02629         switch (o)
02630         {
02631           case 0:
02632             eIndex[0] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[1], i1);
02633             eIndex[1] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[2], i2);
02634             break;
02635           case 1:
02636             eIndex[0] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[0], i1);
02637             eIndex[1] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[2], i2);
02638             break;
02639           case 2:
02640             eIndex[0] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[0], i1);
02641             eIndex[1] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[1], i2);
02642             break;
02643         }
02644         ri.key = static_cast<long long> (o) | static_cast<long long> (eIndex[0]) << 5 | static_cast<long long> (eIndex[1]) << 25 | static_cast<long long> (offset) << 45;
02645         return 1;
02646       }
02647     }
02648 
02650     template<int Degree> int 
02651     Octree<Degree>::GetRootIndex (const TreeOctNode* node, const int& edgeIndex, const int& maxDepth, RootInfo& ri)
02652     {
02653       int c1, c2, f1, f2;
02654       const TreeOctNode *temp, *finest;
02655       int finestIndex;
02656 
02657 
02658       // The assumption is that the super-edge has a root along it.
02659       if (!(MarchingCubes::edgeMask[node->nodeData.mcIndex] & (1 << edgeIndex)))
02660       {
02661         return 0;
02662       }
02663 
02664       Cube::FacesAdjacentToEdge (edgeIndex, f1, f2);
02665 
02666       finest = node;
02667       finestIndex = edgeIndex;
02668       if (node->depth () < maxDepth)
02669       {
02670         temp = node->faceNeighbor (f1);
02671         if (temp && temp->children)
02672         {
02673           finest = temp;
02674           finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f1);
02675         }
02676         else
02677         {
02678           temp = node->faceNeighbor (f2);
02679           if (temp && temp->children)
02680           {
02681             finest = temp;
02682             finestIndex = Cube::FaceReflectEdgeIndex (edgeIndex, f2);
02683           }
02684           else
02685           {
02686             temp = node->edgeNeighbor (edgeIndex);
02687             if (temp && temp->children)
02688             {
02689               finest = temp;
02690               finestIndex = Cube::EdgeReflectEdgeIndex (edgeIndex);
02691             }
02692           }
02693         }
02694       }
02695 
02696       Cube::EdgeCorners (finestIndex, c1, c2);
02697       if (finest->children)
02698       {
02699         if (GetRootIndex (&finest->children[c1], finestIndex, maxDepth, ri))
02700         {
02701           return 1;
02702         }
02703         else if (GetRootIndex (&finest->children[c2], finestIndex, maxDepth, ri))
02704         {
02705           return 1;
02706         }
02707         else
02708         {
02709           return 0;
02710         }
02711       }
02712       else
02713       {
02714         int o, i1, i2;
02715         Cube::FactorEdgeIndex (finestIndex, o, i1, i2);
02716         int d, off[3];
02717         finest->depthAndOffset (d, off);
02718         ri.node = finest;
02719         ri.edgeIndex = finestIndex;
02720         int offset, eIndex[2];
02721         offset = BinaryNode<Real>::Index (d, off[o]);
02722         //initialize to remove warnings
02723         eIndex[0] = eIndex[1] = 0;
02724         switch (o)
02725         {
02726           case 0:
02727             eIndex[0] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[1], i1);
02728             eIndex[1] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[2], i2);
02729             break;
02730           case 1:
02731             eIndex[0] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[0], i1);
02732             eIndex[1] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[2], i2);
02733             break;
02734           case 2:
02735             eIndex[0] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[0], i1);
02736             eIndex[1] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[1], i2);
02737             break;
02738         }
02739         ri.key = static_cast<long long> (o) | static_cast<long long> (eIndex[0]) << 5 | static_cast<long long> (eIndex[1]) << 25 | static_cast<long long> (offset) << 45;
02740         return (1);
02741       }
02742     }
02743 
02745     template<int Degree> int 
02746     Octree<Degree>::GetRootPair (const RootInfo& ri, const int& maxDepth, RootInfo& pair)
02747     {
02748       const TreeOctNode* node = ri.node;
02749       int c1, c2, c;
02750       Cube::EdgeCorners (ri.edgeIndex, c1, c2);
02751       while (node->parent)
02752       {
02753         c = int (node - node->parent->children);
02754         if (c != c1 && c != c2)
02755         {
02756           return 0;
02757         }
02758         if (!MarchingCubes::HasEdgeRoots (node->parent->nodeData.mcIndex, ri.edgeIndex))
02759         {
02760           if (c == c1)
02761           {
02762             return GetRootIndex (&node->parent->children[c2], ri.edgeIndex, maxDepth, pair);
02763           }
02764           else
02765           {
02766             return GetRootIndex (&node->parent->children[c1], ri.edgeIndex, maxDepth, pair);
02767           }
02768         }
02769         node = node->parent;
02770       }
02771       return 0;
02772 
02773     }
02774 
02776     template<int Degree> int 
02777     Octree<Degree>::GetRootIndex (const long long& key, hash_map<long long, int>& boundaryRoots, hash_map<long long, int>* interiorRoots, CoredPointIndex& index)
02778     {
02779       hash_map<long long, int>::iterator rootIter = boundaryRoots.find (key);
02780       if (rootIter != boundaryRoots.end ())
02781       {
02782         index.inCore = 1;
02783         index.index = rootIter->second;
02784         return 1;
02785       }
02786       else if (interiorRoots)
02787       {
02788         rootIter = interiorRoots->find (key);
02789         if (rootIter != interiorRoots->end ())
02790         {
02791           index.inCore = 0;
02792           index.index = rootIter->second;
02793           return 1;
02794         }
02795       }
02796       return 0;
02797     }
02798 
02800     template<int Degree> int 
02801     Octree<Degree>::SetMCRootPositions (
02802         TreeOctNode* node, const int& sDepth, const Real& isoValue,
02803         hash_map<long long, int>& boundaryRoots, hash_map<long long, int>* interiorRoots,
02804         hash_map<long long, std::pair<Real, Point3D<Real> > >& boundaryNormalHash, hash_map<long long, std::pair<Real, Point3D<Real> > >* interiorNormalHash,
02805         std::vector<Point3D<float> >* interiorPositions,
02806         CoredMeshData* mesh, const int& nonLinearFit)
02807     {
02808       Point3D<Real> position;
02809       int i, j, k, eIndex;
02810       RootInfo ri;
02811       int count = 0;
02812       if (!MarchingCubes::HasRoots (node->nodeData.mcIndex))
02813       {
02814         return 0;
02815       }
02816       for (i = 0; i < DIMENSION; i++)
02817       {
02818         for (j = 0; j < 2; j++)
02819         {
02820           for (k = 0; k < 2; k++)
02821           {
02822             long long key;
02823             eIndex = Cube::EdgeIndex (i, j, k);
02824             if (GetRootIndex (node, eIndex, fData.depth, ri))
02825             {
02826               key = ri.key;
02827               if (!interiorRoots || IsBoundaryEdge (node, i, j, k, sDepth))
02828               {
02829                 if (boundaryRoots.find (key) == boundaryRoots.end ())
02830                 {
02831                   GetRoot (ri, isoValue, fData.depth, position, boundaryNormalHash, NULL, nonLinearFit);
02832                   mesh->inCorePoints.push_back (position);
02833                   boundaryRoots[key] = int (mesh->inCorePoints.size ()) - 1;
02834                   count++;
02835                 }
02836               }
02837               else
02838               {
02839                 if (interiorRoots->find (key) == interiorRoots->end ())
02840                 {
02841                   GetRoot (ri, isoValue, fData.depth, position, *interiorNormalHash, NULL, nonLinearFit);
02842                   (*interiorRoots)[key] = mesh->addOutOfCorePoint (position);
02843                   interiorPositions->push_back (position);
02844                   count++;
02845                 }
02846               }
02847             }
02848           }
02849         }
02850       }
02851       return count;
02852     }
02853 
02855     template<int Degree> int 
02856     Octree<Degree>::SetBoundaryMCRootPositions (
02857         const int& sDepth, const Real& isoValue, hash_map<long long, int>& boundaryRoots, hash_map<long long, std::pair<Real, Point3D<Real> > >& boundaryNormalHash,
02858                                   CoredMeshData* mesh, const int& nonLinearFit)
02859     {
02860       Point3D<Real> position;
02861       int i, j, k, eIndex, hits = 0;
02862       RootInfo ri;
02863       int count = 0;
02864       TreeOctNode* node;
02865 
02866       node = tree.nextLeaf ();
02867       while (node)
02868       {
02869         if (MarchingCubes::HasRoots (node->nodeData.mcIndex))
02870         {
02871           hits = 0;
02872           for (i = 0; i < DIMENSION; i++)
02873           {
02874             for (j = 0; j < 2; j++)
02875             {
02876               for (k = 0; k < 2; k++)
02877               {
02878                 if (IsBoundaryEdge (node, i, j, k, sDepth))
02879                 {
02880                   hits++;
02881                   long long key;
02882                   eIndex = Cube::EdgeIndex (i, j, k);
02883                   if (GetRootIndex (node, eIndex, fData.depth, ri))
02884                   {
02885                     key = ri.key;
02886                     if (boundaryRoots.find (key) == boundaryRoots.end ())
02887                     {
02888                       GetRoot (ri, isoValue, fData.depth, position, boundaryNormalHash, NULL, nonLinearFit);
02889                       mesh->inCorePoints.push_back (position);
02890                       boundaryRoots[key] = int (mesh->inCorePoints.size ()) - 1;
02891                       count++;
02892                     }
02893                   }
02894                 }
02895               }
02896             }
02897           }
02898         }
02899         if (hits)
02900         {
02901           node = tree.nextLeaf (node);
02902         }
02903         else
02904         {
02905           node = tree.nextBranch (node);
02906         }
02907       }
02908       return count;
02909     }
02910 
02912     template<int Degree> void 
02913     Octree<Degree>::GetMCIsoEdges (TreeOctNode* node, hash_map<long long, int>& /*boundaryRoots*/, hash_map<long long, int>* /*interiorRoots*/, const int& sDepth,
02914                                    std::vector<std::pair<long long, long long> >& edges)
02915     {
02916       TreeOctNode* temp;
02917       int count = 0; //,tris = 0;
02918       int isoTri[DIMENSION * MarchingCubes::MAX_TRIANGLES];
02919       FaceEdgesFunction fef;
02920       int ref, fIndex;
02921       hash_map<long long, std::pair<RootInfo, int> >::iterator iter;
02922       hash_map<long long, std::pair<RootInfo, int> > vertexCount;
02923 
02924       fef.edges = &edges;
02925       fef.maxDepth = fData.depth;
02926       fef.vertexCount = &vertexCount;
02927       count = MarchingCubes::AddTriangleIndices (node->nodeData.mcIndex, isoTri);
02928       for (fIndex = 0; fIndex < Cube::NEIGHBORS; fIndex++)
02929       {
02930         ref = Cube::FaceReflectFaceIndex (fIndex, fIndex);
02931         fef.fIndex = ref;
02932         temp = node->faceNeighbor (fIndex);
02933         // If the face neighbor exists and has higher resolution than the current node,
02934         // get the iso-curve from the neighbor
02935         if (temp && temp->children && !IsBoundaryFace (node, fIndex, sDepth))
02936         {
02937           temp->processNodeFaces (temp, &fef, ref);
02938         }
02939           // Otherwise, get it from the node
02940         else
02941         {
02942           RootInfo ri1, ri2;
02943           for (int j = 0; j < count; j++)
02944           {
02945             for (int k = 0; k < 3; k++)
02946             {
02947               if (fIndex == Cube::FaceAdjacentToEdges (isoTri[j * 3 + k], isoTri[j * 3 + ((k + 1) % 3)]))
02948               {
02949                 if (GetRootIndex (node, isoTri[j * 3 + k], fData.depth, ri1) && GetRootIndex (node, isoTri[j * 3 + ((k + 1) % 3)], fData.depth, ri2))
02950                 {
02951                   edges.push_back (std::pair<long long, long long> (ri1.key, ri2.key));
02952                   iter = vertexCount.find (ri1.key);
02953                   if (iter == vertexCount.end ())
02954                   {
02955                     vertexCount[ri1.key].first = ri1;
02956                     vertexCount[ri1.key].second = 0;
02957                   }
02958                   iter = vertexCount.find (ri2.key);
02959                   if (iter == vertexCount.end ())
02960                   {
02961                     vertexCount[ri2.key].first = ri2;
02962                     vertexCount[ri2.key].second = 0;
02963                   }
02964                   vertexCount[ri1.key].second++;
02965                   vertexCount[ri2.key].second--;
02966                 }
02967                 else
02968                 {
02969                   fprintf (stderr, "Bad Edge 1: %d %d\n", int (ri1.key), int (ri2.key));
02970                 }
02971               }
02972             }
02973           }
02974         }
02975       }
02976       for (int i = 0; i<int (edges.size ()); i++)
02977       {
02978         iter = vertexCount.find (edges[i].first);
02979         if (iter == vertexCount.end ())
02980           std::cerr << "Could not find vertex: " << edges[i].first << std::endl;
02981         else if (vertexCount[edges[i].first].second)
02982         {
02983           RootInfo ri;
02984           GetRootPair (vertexCount[edges[i].first].first, fData.depth, ri);
02985           iter = vertexCount.find (ri.key);
02986           if (iter == vertexCount.end ())
02987           {
02988             printf ("Vertex pair not in list\n");
02989           }
02990           else
02991           {
02992             edges.push_back (std::pair<long long, long long> (ri.key, edges[i].first));
02993             vertexCount[ri.key].second++;
02994             vertexCount[edges[i].first].second--;
02995           }
02996         }
02997 
02998         iter = vertexCount.find (edges[i].second);
02999         if (iter == vertexCount.end ())
03000           std::cerr << "Could not find vertex: " << edges[i].second << std::endl;
03001         else if (vertexCount[edges[i].second].second)
03002         {
03003           RootInfo ri;
03004           GetRootPair (vertexCount[edges[i].second].first, fData.depth, ri);
03005           iter = vertexCount.find (ri.key);
03006           if (iter == vertexCount.end ())
03007           {
03008             printf ("Vertex pair not in list\n");
03009           }
03010           else
03011           {
03012             edges.push_back (std::pair<long long, long long> (edges[i].second, ri.key));
03013             vertexCount[edges[i].second].second++;
03014             vertexCount[ri.key].second--;
03015           }
03016         }
03017       }
03018     }
03019 
03020     template<int Degree>
03021     int Octree<Degree>
03022     ::GetMCIsoTriangles (TreeOctNode* node, CoredMeshData* mesh, hash_map<long long, int>& boundaryRoots,
03023                          hash_map<long long, int>* interiorRoots, std::vector<Point3D<float> >* interiorPositions, const int& offSet, const int& sDepth, bool addBarycenter, bool polygonMesh)
03024     {
03025       int tris = 0;
03026       std::vector<std::pair<long long, long long> > edges;
03027       std::vector<std::vector<std::pair<long long, long long> > > edgeLoops;
03028       GetMCIsoEdges (node, boundaryRoots, interiorRoots, sDepth, edges);
03029 
03030       GetEdgeLoops (edges, edgeLoops);
03031       for (int i = 0; i<int (edgeLoops.size ()); i++)
03032       {
03033         CoredPointIndex p;
03034         std::vector<CoredPointIndex> edgeIndices;
03035         for (int j = 0; j<int (edgeLoops[i].size ()); j++)
03036         {
03037           if (!GetRootIndex (edgeLoops[i][j].first, boundaryRoots, interiorRoots, p))
03038           {
03039             printf ("Bad Point Index\n");
03040           }
03041           else
03042           {
03043             edgeIndices.push_back (p);
03044           }
03045         }
03046         tris += AddTriangles (mesh, edgeIndices, interiorPositions, offSet, addBarycenter, polygonMesh);
03047       }
03048       return tris;
03049     }
03050 
03052     template<int Degree> int 
03053     Octree<Degree>::GetEdgeLoops (std::vector<std::pair<long long, long long> >& edges, std::vector<std::vector<std::pair<long long, long long> > >& loops)
03054     {
03055       int loopSize = 0;
03056       long long frontIdx, backIdx;
03057       std::pair<long long, long long> e, temp;
03058       loops.clear ();
03059 
03060       while (edges.size ())
03061       {
03062         std::vector<std::pair<long long, long long> > front, back;
03063         e = edges[0];
03064         loops.resize (loopSize + 1);
03065         edges[0] = edges[edges.size () - 1];
03066         edges.pop_back ();
03067         frontIdx = e.second;
03068         backIdx = e.first;
03069         for (int j = int (edges.size ()) - 1; j >= 0; j--)
03070         {
03071           if (edges[j].first == frontIdx || edges[j].second == frontIdx)
03072           {
03073             if (edges[j].first == frontIdx)
03074             {
03075               temp = edges[j];
03076             }
03077             else
03078             {
03079               temp.first = edges[j].second;
03080               temp.second = edges[j].first;
03081             }
03082             frontIdx = temp.second;
03083             front.push_back (temp);
03084             edges[j] = edges[edges.size () - 1];
03085             edges.pop_back ();
03086             j = int (edges.size ());
03087           }
03088           else if (edges[j].first == backIdx || edges[j].second == backIdx)
03089           {
03090             if (edges[j].second == backIdx)
03091             {
03092               temp = edges[j];
03093             }
03094             else
03095             {
03096               temp.first = edges[j].second;
03097               temp.second = edges[j].first;
03098             }
03099             backIdx = temp.first;
03100             back.push_back (temp);
03101             edges[j] = edges[edges.size () - 1];
03102             edges.pop_back ();
03103             j = int (edges.size ());
03104           }
03105         }
03106         for (int j = int (back.size ()) - 1; j >= 0; j--)
03107         {
03108           loops[loopSize].push_back (back[j]);
03109         }
03110         loops[loopSize].push_back (e);
03111         for (int j = 0; j<int (front.size ()); j++)
03112         {
03113           loops[loopSize].push_back (front[j]);
03114         }
03115         loopSize++;
03116       }
03117       return int (loops.size ());
03118     }
03119 
03121     template<int Degree> int 
03122     Octree<Degree>::AddTriangles (CoredMeshData* mesh, std::vector<CoredPointIndex> edges[3], std::vector<Point3D<float> >* interiorPositions, const int& offSet)
03123     {
03124       std::vector<CoredPointIndex> e;
03125       for (int i = 0; i < 3; i++)
03126       {
03127         for (size_t j = 0; j < edges[i].size (); j++)
03128         {
03129           e.push_back (edges[i][j]);
03130         }
03131       }
03132       return AddTriangles (mesh, e, interiorPositions, offSet);
03133     }
03134 
03136     template<int Degree> int 
03137     Octree<Degree>::AddTriangles (CoredMeshData* mesh, std::vector<CoredPointIndex>& edges, std::vector<Point3D<float> >* interiorPositions, const int& offSet, bool addBarycenter, bool polygonMesh)
03138     {
03139       if (polygonMesh)
03140       {
03141         std::vector< CoredVertexIndex > vertices (edges.size ());
03142         for (size_t i = 0; i < edges.size (); i++)
03143         {
03144           vertices[i].idx = edges[i].index;
03145           vertices[i].inCore = edges[i].inCore;
03146         }
03147         mesh->addPolygon (vertices);
03148         return 1;
03149       }
03150       if (edges.size () > 3)
03151       {
03152 #if 1
03153         bool isCoplanar = false;
03154 
03155         for (size_t i = 0; i < edges.size (); i++)
03156           for (size_t j = 0; j < i; j++)
03157             if ((i + 1) % edges.size () != j && (j + 1) % edges.size () != i)
03158             {
03159               Point3D< Real > v1, v2;
03160               if (edges[i].inCore) for (int k = 0; k < 3; k++) v1.coords[k] = mesh->inCorePoints[ edges[i].index ].coords[k];
03161               else for (int k = 0; k < 3; k++) v1.coords[k] = (*interiorPositions)[ edges[i].index - offSet ].coords[k];
03162               if (edges[j].inCore) for (int k = 0; k < 3; k++) v2.coords[k] = mesh->inCorePoints[ edges[j].index ].coords[k];
03163               else for (int k = 0; k < 3; k++) v2.coords[k] = (*interiorPositions)[ edges[j].index - offSet ].coords[k];
03164               for (int k = 0; k < 3; k++) if (v1.coords[k] == v2.coords[k]) isCoplanar = true;
03165             }
03166         if (addBarycenter && isCoplanar)
03167 #else
03168         if (addBarycenter)
03169 #endif
03170         {
03171           Point3D< Real > c;
03172           c.coords[0] = c.coords[1] = c.coords[2] = 0;
03173           for (int i = 0; i<int (edges.size ()); i++)
03174           {
03175             Point3D<Real> p;
03176             if (edges[i].inCore) for (int j = 0; j < 3; j++) p.coords[j] = mesh->inCorePoints[edges[i].index].coords[j];
03177             else for (int j = 0; j < 3; j++) p.coords[j] = (*interiorPositions)[edges[i].index - offSet].coords[j];
03178             c.coords[0] += p.coords[0], c.coords[1] += p.coords[1], c.coords[2] += p.coords[2];
03179           }
03180           c.coords[0] /= static_cast<Real> (edges.size ());
03181           c.coords[1] /= static_cast<Real> (edges.size ()); 
03182           c.coords[2] /= static_cast<Real> (edges.size ());
03183           int cIdx = mesh->addOutOfCorePoint (c);
03184           for (int i = 0; i<int (edges.size ()); i++)
03185           {
03186             std::vector< CoredVertexIndex > vertices (3);
03187             vertices[0].idx = edges[i].index;
03188             vertices[1].idx = edges[ (i + 1) % edges.size ()].index;
03189             vertices[2].idx = cIdx;
03190             vertices[0].inCore = edges[i ].inCore;
03191             vertices[1].inCore = edges[ (i + 1) % edges.size ()].inCore;
03192             vertices[2].inCore = 0;
03193             mesh->addPolygon (vertices);
03194           }
03195           return (static_cast<int> (edges.size ()));
03196         }
03197         else
03198         {
03199           Triangulation<float> t;
03200 
03201           // Add the points to the triangulation
03202           for (int i = 0; i<int (edges.size ()); i++)
03203           {
03204             Point3D<Real> p;
03205             if (edges[i].inCore)
03206             {
03207               for (int j = 0; j < 3; j++)
03208               {
03209                 p.coords[j] = mesh->inCorePoints[edges[i].index].coords[j];
03210               }
03211             }
03212             else
03213             {
03214               for (int j = 0; j < 3; j++)
03215               {
03216                 p.coords[j] = (*interiorPositions)[edges[i].index - offSet].coords[j];
03217               }
03218             }
03219             t.points.push_back (p);
03220           }
03221 
03222           // Create a fan triangulation
03223           for (int i = 1; i<int (edges.size ()) - 1; i++)
03224           {
03225             t.addTriangle (0, i, i + 1);
03226           }
03227 
03228           // Minimize
03229           while (1)
03230           {
03231             size_t i;
03232             for (i = 0; i < t.edges.size (); i++)
03233             {
03234               if (t.flipMinimize (static_cast<int> (i)))
03235                 break;
03236             }
03237             if (i == t.edges.size ())
03238               break;
03239           }
03240           // Add the triangles to the mesh
03241           for (int i = 0; i<int (t.triangles.size ()); i++)
03242           {
03243             std::vector< CoredVertexIndex > vertices (3);
03244             int idx[3];
03245             t.factor (i, idx[0], idx[1], idx[2]);
03246             for (int j = 0; j < 3; j++)
03247             {
03248               vertices[j].idx = edges[ idx[j] ].index;
03249               vertices[j].inCore = edges[ idx[j] ].inCore;
03250             }
03251             mesh->addPolygon (vertices);
03252           }
03253         }
03254       }
03255       else if (edges.size () == 3)
03256       {
03257         std::vector< CoredVertexIndex > vertices (3);
03258         for (int i = 0; i < 3; i++)
03259         {
03260           vertices[i].idx = edges[i].index;
03261           vertices[i].inCore = edges[i].inCore;
03262         }
03263         mesh->addPolygon (vertices);
03264       }
03265       return int (edges.size ()) - 2;
03266     }
03268     // VertexData //
03270 
03272     long long
03273     VertexData::CenterIndex (const TreeOctNode* node, const int& maxDepth)
03274     {
03275       int idx[DIMENSION];
03276       return CenterIndex (node, maxDepth, idx);
03277     }
03278 
03280     long long
03281     VertexData::CenterIndex (const TreeOctNode* node, const int& maxDepth, int idx[DIMENSION])
03282     {
03283       int d, o[3];
03284       node->depthAndOffset (d, o);
03285       for (int i = 0; i < DIMENSION; i++)
03286       {
03287         idx[i] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d + 1, o[i] << 1, 1);
03288       }
03289       return (static_cast<long long> (idx[0]) | static_cast<long long> (idx[1]) << 15 | static_cast<long long> (idx[2]) << 30);
03290     }
03291 
03293     long long
03294     VertexData::CenterIndex (const int& depth, const int offSet[DIMENSION], const int& maxDepth, int idx[DIMENSION])
03295     {
03296       for (int i = 0; i < DIMENSION; i++)
03297       {
03298         idx[i] = BinaryNode<Real>::CornerIndex (maxDepth + 1, depth + 1, offSet[i] << 1, 1);
03299       }
03300       return (static_cast<long long> (idx[0]) | static_cast<long long> (idx[1]) << 15 | static_cast<long long> (idx[2]) << 30);
03301     }
03302 
03304     long long
03305     VertexData::CornerIndex (const TreeOctNode* node, const int& cIndex, const int& maxDepth)
03306     {
03307       int idx[DIMENSION];
03308       return CornerIndex (node, cIndex, maxDepth, idx);
03309     }
03310 
03312     long long
03313     VertexData::CornerIndex (const TreeOctNode* node, const int& cIndex, const int& maxDepth, int idx[DIMENSION])
03314     {
03315       int x[DIMENSION];
03316       Cube::FactorCornerIndex (cIndex, x[0], x[1], x[2]);
03317       int d, o[3];
03318       node->depthAndOffset (d, o);
03319       for (int i = 0; i < DIMENSION; i++)
03320       {
03321         idx[i] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, o[i], x[i]);
03322       }
03323       return (static_cast<long long> (idx[0]) | static_cast<long long> (idx[1]) << 15 | static_cast<long long> (idx[2]) << 30);
03324     }
03325 
03327     long long
03328     VertexData::CornerIndex (const int& depth, const int offSet[DIMENSION], const int& cIndex, const int& maxDepth, int idx[DIMENSION])
03329     {
03330       int x[DIMENSION];
03331       Cube::FactorCornerIndex (cIndex, x[0], x[1], x[2]);
03332       for (int i = 0; i < DIMENSION; i++)
03333       {
03334         idx[i] = BinaryNode<Real>::CornerIndex (maxDepth + 1, depth, offSet[i], x[i]);
03335       }
03336       return (static_cast<long long> (idx[0]) | static_cast<long long> (idx[1]) << 15 | static_cast<long long> (idx[2]) << 30);
03337     }
03338 
03340     long long
03341     VertexData::FaceIndex (const TreeOctNode* node, const int& fIndex, const int& maxDepth)
03342     {
03343       int idx[DIMENSION];
03344       return FaceIndex (node, fIndex, maxDepth, idx);
03345     }
03346 
03348     long long
03349     VertexData::FaceIndex (const TreeOctNode* node, const int& fIndex, const int& maxDepth, int idx[DIMENSION])
03350     {
03351       int dir, offset;
03352       Cube::FactorFaceIndex (fIndex, dir, offset);
03353       int d, o[3];
03354       node->depthAndOffset (d, o);
03355       for (int i = 0; i < DIMENSION; i++)
03356       {
03357         idx[i] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d + 1, o[i] << 1, 1);
03358       }
03359       idx[dir] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, o[dir], offset);
03360       return (static_cast<long long> (idx[0]) | static_cast<long long> (idx[1]) << 15 | static_cast<long long> (idx[2]) << 30);
03361     }
03362 
03364     long long
03365     VertexData::EdgeIndex (const TreeOctNode* node, const int& eIndex, const int& maxDepth)
03366     {
03367       int idx[DIMENSION];
03368       return EdgeIndex (node, eIndex, maxDepth, idx);
03369     }
03370 
03372     long long
03373     VertexData::EdgeIndex (const TreeOctNode* node, const int& eIndex, const int& maxDepth, int idx[DIMENSION])
03374     {
03375       int o, i1, i2;
03376       int d, off[3];
03377       node->depthAndOffset (d, off);
03378       for (int i = 0; i < DIMENSION; i++)
03379       {
03380         idx[i] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d + 1, off[i] << 1, 1);
03381       }
03382       Cube::FactorEdgeIndex (eIndex, o, i1, i2);
03383       switch (o)
03384       {
03385         case 0:
03386           idx[1] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[1], i1);
03387           idx[2] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[2], i2);
03388           break;
03389         case 1:
03390           idx[0] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[0], i1);
03391           idx[2] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[2], i2);
03392           break;
03393         case 2:
03394           idx[0] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[0], i1);
03395           idx[1] = BinaryNode<Real>::CornerIndex (maxDepth + 1, d, off[1], i2);
03396           break;
03397       };
03398       return (static_cast<long long> (idx[0]) | static_cast<long long> (idx[1]) << 15 | static_cast<long long> (idx[2]) << 30);
03399     }
03400   }
03401 }
03402 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:44