Point Cloud Library (PCL)  1.9.1-dev
multi_grid_octree_data.hpp
1 /*
2 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6 are permitted provided that the following conditions are met:
7 
8 Redistributions of source code must retain the above copyright notice, this list of
9 conditions and the following disclaimer. Redistributions in binary form must reproduce
10 the above copyright notice, this list of conditions and the following disclaimer
11 in the documentation and/or other materials provided with the distribution.
12 
13 Neither the name of the Johns Hopkins University nor the names of its contributors
14 may be used to endorse or promote products derived from this software without specific
15 prior written permission.
16 
17 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26 DAMAGE.
27 */
28 
29 #include <unordered_map>
30 
31 #include "poisson_exceptions.h"
32 #include "octree_poisson.h"
33 #include "mat.h"
34 
35 #if defined WIN32 || defined _WIN32
36  #if !defined __MINGW32__
37  #include <intrin.h>
38  #endif
39 #endif
40 
41 
42 #define ITERATION_POWER 1.0/3
43 #define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12
44 #define SPLAT_ORDER 2
45 
46 namespace pcl
47 {
48  namespace poisson
49  {
50 
52  const Real EPSILON=Real(1e-6);
53  const Real ROUND_EPS=Real(1e-5);
54 
55  void atomicOr(volatile int& dest, int value)
56  {
57 #if defined _WIN32 && !defined __MINGW32__
58  #if defined (_M_IX86)
59  _InterlockedOr( (long volatile*)&dest, value );
60  #else
61  InterlockedOr( (long volatile*)&dest , value );
62  #endif
63 #else // !_WIN32 || __MINGW32__
64  #pragma omp atomic
65  dest |= value;
66 #endif // _WIN32 && !__MINGW32__
67  }
68 
69 
70  /////////////////////
71  // SortedTreeNodes //
72  /////////////////////
74  {
75  nodeCount=NULL;
76  treeNodes=NULL;
77  maxDepth=0;
78  }
80  if( nodeCount ) delete[] nodeCount;
81  if( treeNodes ) delete[] treeNodes;
82  nodeCount = NULL;
83  treeNodes = NULL;
84  }
85 
87  {
88  if( nodeCount ) delete[] nodeCount;
89  if( treeNodes ) delete[] treeNodes;
90  maxDepth = root.maxDepth()+1;
91  nodeCount = new int[ maxDepth+1 ];
92  treeNodes = new TreeOctNode*[ root.nodes() ];
93 
94  nodeCount[0] = 0 , nodeCount[1] = 1;
95  treeNodes[0] = &root;
96  for( int d=1 ; d<maxDepth ; d++ )
97  {
98  nodeCount[d+1] = nodeCount[d];
99  for( int i=nodeCount[d-1] ; i<nodeCount[d] ; i++ )
100  {
101  TreeOctNode* temp = treeNodes[i];
102  if( temp->children ) for( int c=0 ; c<8 ; c++ ) treeNodes[ nodeCount[d+1]++ ] = temp->children + c;
103  }
104  }
105  for( int i=0 ; i<nodeCount[maxDepth] ; i++ ) treeNodes[i]->nodeData.nodeIndex = i;
106  }
108  const SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::operator[] ( const TreeOctNode* node ) const { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
110  const SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::cornerIndices( const TreeOctNode* node ) const { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
111  void SortedTreeNodes::setCornerTable( CornerTableData& cData , const TreeOctNode* rootNode , int maxDepth , int threads ) const
112  {
113  if( threads<=0 ) threads = 1;
114  // The vector of per-depth node spans
115  std::vector< std::pair< int , int > > spans( this->maxDepth , std::pair< int , int >( -1 , -1 ) );
116  int minDepth , off[3];
117  rootNode->depthAndOffset( minDepth , off );
118  cData.offsets.resize( this->maxDepth , -1 );
119  int nodeCount = 0;
120  {
121  int start=rootNode->nodeData.nodeIndex , end=start;
122  for( int d=minDepth ; d<=maxDepth ; d++ )
123  {
124  spans[d] = std::pair< int , int >( start , end+1 );
125  cData.offsets[d] = nodeCount - spans[d].first;
126  nodeCount += spans[d].second - spans[d].first;
127  if( d<maxDepth )
128  {
129  while( start< end && !treeNodes[start]->children ) start++;
130  while( end> start && !treeNodes[end ]->children ) end--;
131  if( start==end && !treeNodes[start]->children ) break;
132  start = treeNodes[start]->children[0].nodeData.nodeIndex;
133  end = treeNodes[end ]->children[7].nodeData.nodeIndex;
134  }
135  }
136  }
137  cData.cTable.resize( nodeCount );
138  std::vector< int > count( threads );
139 #pragma omp parallel for num_threads( threads )
140  for( int t=0 ; t<threads ; t++ )
141  {
142  TreeOctNode::ConstNeighborKey3 neighborKey;
143  neighborKey.set( maxDepth );
144  int offset = nodeCount * t * Cube::CORNERS;
145  count[t] = 0;
146  for( int d=minDepth ; d<=maxDepth ; d++ )
147  {
148  int start = spans[d].first , end = spans[d].second , width = end-start;
149  for( int i=start + (width*t)/threads ; i<start + (width*(t+1))/threads ; i++ )
150  {
151  TreeOctNode* node = treeNodes[i];
152  if( d<maxDepth && node->children ) continue;
153  const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , minDepth );
154  for( int c=0 ; c<Cube::CORNERS ; c++ ) // Iterate over the cell's corners
155  {
156  bool cornerOwner = true;
157  int x , y , z;
158  int ac = Cube::AntipodalCornerIndex( c ); // The index of the node relative to the corner
159  Cube::FactorCornerIndex( c , x , y , z );
160  for( int cc=0 ; cc<Cube::CORNERS ; cc++ ) // Iterate over the corner's cells
161  {
162  int xx , yy , zz;
163  Cube::FactorCornerIndex( cc , xx , yy , zz );
164  xx += x , yy += y , zz += z;
165  if( neighbors.neighbors[xx][yy][zz] )
166  if( cc<ac || ( d<maxDepth && neighbors.neighbors[xx][yy][zz]->children ) )
167  {
168  int _d , _off[3];
169  neighbors.neighbors[xx][yy][zz]->depthAndOffset( _d , _off );
170  _off[0] >>= (d-minDepth) , _off[1] >>= (d-minDepth) , _off[2] >>= (d-minDepth);
171  if( _off[0]==off[0] && _off[1]==off[1] && _off[2]==off[2] )
172  {
173  cornerOwner = false;
174  break;
175  }
176  else fprintf( stderr , "[WARNING] How did we leave the subtree?\n" );
177  }
178  }
179  if( cornerOwner )
180  {
181  const TreeOctNode* n = node;
182  int d = n->depth();
183  do
184  {
185  const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.neighbors[d];
186  // Set all the corner indices at the current depth
187  for( int cc=0 ; cc<Cube::CORNERS ; cc++ )
188  {
189  int xx , yy , zz;
190  Cube::FactorCornerIndex( cc , xx , yy , zz );
191  xx += x , yy += y , zz += z;
192  if( neighborKey.neighbors[d].neighbors[xx][yy][zz] )
193  cData[ neighbors.neighbors[xx][yy][zz] ][ Cube::AntipodalCornerIndex(cc) ] = count[t] + offset;
194  }
195  // If we are not at the root and the parent also has the corner
196  if( d==minDepth || n!=(n->parent->children+c) ) break;
197  n = n->parent;
198  d--;
199  }
200  while( 1 );
201  count[t]++;
202  }
203  }
204  }
205  }
206  }
207  cData.cCount = 0;
208  std::vector< int > offsets( threads+1 );
209  offsets[0] = 0;
210  for (int t = 0; t < threads; t++)
211  {
212  cData.cCount += count[t];
213  offsets[t + 1] = offsets[t] + count[t];
214  }
215 
216  unsigned int paralellExceptionCount = 0;
217 #pragma omp parallel for num_threads( threads )
218  for (int t = 0; t < threads; t++)
219  {
220  for (int d = minDepth; d <= maxDepth; d++)
221  {
222  int start = spans[d].first, end = spans[d].second, width = end - start;
223  for (int i = start + (width*t) / threads; i < start + (width*(t + 1)) / threads; i++)
224  {
225  for (int c = 0; c < Cube::CORNERS; c++)
226  {
227  int& idx = cData[ treeNodes[i] ][c];
228  if ( idx<0 )
229  {
230 #pragma omp critical
231  {
232  // found unindexed corner
233  ++paralellExceptionCount;
234  }
235  }
236  else
237  {
238  int div = idx / ( nodeCount*Cube::CORNERS );
239  int rem = idx % ( nodeCount*Cube::CORNERS );
240  idx = rem + offsets[div];
241  }
242  }
243  }
244  }
245  }
246 
247  if (paralellExceptionCount > 0)
248  POISSON_THROW_EXCEPTION (pcl::poisson::PoissonOpenMPException, "Found " << paralellExceptionCount << " unindexed corner nodes during openMP loop execution.");
249  }
250  int SortedTreeNodes::getMaxCornerCount( const TreeOctNode* rootNode , int depth , int maxDepth , int threads ) const
251  {
252  if( threads<=0 ) threads = 1;
253  int res = 1<<depth;
254  std::vector< std::vector< int > > cornerCount( threads );
255  for( int t=0 ; t<threads ; t++ ) cornerCount[t].resize( res*res*res , 0 );
256 
257 #pragma omp parallel for num_threads( threads )
258  for( int t=0 ; t<threads ; t++ )
259  {
260  std::vector< int >& _cornerCount = cornerCount[t];
261  TreeOctNode::ConstNeighborKey3 neighborKey;
262  neighborKey.set( maxDepth );
263  int start = nodeCount[depth] , end = nodeCount[maxDepth+1] , range = end-start;
264  for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ )
265  {
266  TreeOctNode* node = treeNodes[start+i];
267  int d , off[3];
268  node->depthAndOffset( d , off );
269  if( d<maxDepth && node->children ) continue;
270 
271  const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , depth );
272  for( int c=0 ; c<Cube::CORNERS ; c++ ) // Iterate over the cell's corners
273  {
274  bool cornerOwner = true;
275  int x , y , z;
276  int ac = Cube::AntipodalCornerIndex( c ); // The index of the node relative to the corner
277  Cube::FactorCornerIndex( c , x , y , z );
278  for( int cc=0 ; cc<Cube::CORNERS ; cc++ ) // Iterate over the corner's cells
279  {
280  int xx , yy , zz;
281  Cube::FactorCornerIndex( cc , xx , yy , zz );
282  xx += x , yy += y , zz += z;
283  if( neighbors.neighbors[xx][yy][zz] )
284  if( cc<ac || ( d<maxDepth && neighbors.neighbors[xx][yy][zz]->children ) )
285  {
286  cornerOwner = false;
287  break;
288  }
289  }
290  if( cornerOwner ) _cornerCount[ ( ( off[0]>>(d-depth) ) * res * res) + ( ( off[1]>>(d-depth) ) * res) + ( off[2]>>(d-depth) ) ]++;
291  }
292  }
293  }
294  int maxCount = 0;
295  for( int i=0 ; i<res*res*res ; i++ )
296  {
297  int c = 0;
298  for( int t=0 ; t<threads ; t++ ) c += cornerCount[t][i];
299  maxCount = std::max< int >( maxCount , c );
300  }
301  return maxCount;
302  }
304  const SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::operator[] ( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
306  const SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::edgeIndices( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
307  void SortedTreeNodes::setEdgeTable( EdgeTableData& eData , const TreeOctNode* rootNode , int maxDepth , int threads )
308  {
309  if( threads<=0 ) threads = 1;
310  std::vector< std::pair< int , int > > spans( this->maxDepth , std::pair< int , int >( -1 , -1 ) );
311 
312  int minDepth , off[3];
313  rootNode->depthAndOffset( minDepth , off );
314  eData.offsets.resize( this->maxDepth , -1 );
315  int nodeCount = 0;
316  {
317  int start=rootNode->nodeData.nodeIndex , end=start;
318  for( int d=minDepth ; d<=maxDepth ; d++ )
319  {
320  spans[d] = std::pair< int , int >( start , end+1 );
321  eData.offsets[d] = nodeCount - spans[d].first;
322  nodeCount += spans[d].second - spans[d].first;
323  if( d<maxDepth )
324  {
325  while( start< end && !treeNodes[start]->children ) start++;
326  while( end> start && !treeNodes[end ]->children ) end--;
327  if( start==end && !treeNodes[start]->children ) break;
328  start = treeNodes[start]->children[0].nodeData.nodeIndex;
329  end = treeNodes[end ]->children[7].nodeData.nodeIndex;
330  }
331  }
332  }
333  eData.eTable.resize( nodeCount );
334  std::vector< int > count( threads );
335 #pragma omp parallel for num_threads( threads )
336  for( int t=0 ; t<threads ; t++ )
337  {
338  TreeOctNode::ConstNeighborKey3 neighborKey;
339  neighborKey.set( maxDepth );
340  int offset = nodeCount * t * Cube::EDGES;
341  count[t] = 0;
342  for( int d=minDepth ; d<=maxDepth ; d++ )
343  {
344  int start = spans[d].first , end = spans[d].second , width = end-start;
345  for( int i=start + (width*t)/threads ; i<start + (width*(t+1))/threads ; i++ )
346  {
347  TreeOctNode* node = treeNodes[i];
348  const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , minDepth );
349 
350  for( int e=0 ; e<Cube::EDGES ; e++ )
351  {
352  bool edgeOwner = true;
353  int o , i , j;
354  Cube::FactorEdgeIndex( e , o , i , j );
356  for( int cc=0 ; cc<Square::CORNERS ; cc++ )
357  {
358  int ii , jj , x , y , z;
359  Square::FactorCornerIndex( cc , ii , jj );
360  ii += i , jj += j;
361  switch( o )
362  {
363  case 0: y = ii , z = jj , x = 1 ; break;
364  case 1: x = ii , z = jj , y = 1 ; break;
365  case 2: x = ii , y = jj , z = 1 ; break;
366  }
367  if( neighbors.neighbors[x][y][z] && cc<ac ) { edgeOwner = false ; break; }
368  }
369  if( edgeOwner )
370  {
371  // Set all edge indices
372  for( int cc=0 ; cc<Square::CORNERS ; cc++ )
373  {
374  int ii , jj , aii , ajj , x , y , z;
375  Square::FactorCornerIndex( cc , ii , jj );
377  ii += i , jj += j;
378  switch( o )
379  {
380  case 0: y = ii , z = jj , x = 1 ; break;
381  case 1: x = ii , z = jj , y = 1 ; break;
382  case 2: x = ii , y = jj , z = 1 ; break;
383  }
384  if( neighbors.neighbors[x][y][z] )
385  eData[ neighbors.neighbors[x][y][z] ][ Cube::EdgeIndex( o , aii , ajj ) ] = count[t]+offset;
386  }
387  count[t]++;
388  }
389  }
390  }
391  }
392  }
393  eData.eCount = 0;
394  std::vector< int > offsets( threads+1 );
395  offsets[0] = 0;
396  for (int t = 0; t < threads; t++)
397  {
398  eData.eCount += count[t];
399  offsets[t + 1] = offsets[t] + count[t];
400  }
401 
402  unsigned int paralellExceptionCount = 0;
403 #pragma omp parallel for num_threads( threads )
404  for (int t = 0; t < threads; t++)
405  {
406  for (int d = minDepth; d <= maxDepth; d++)
407  {
408  int start = spans[d].first, end = spans[d].second, width = end - start;
409  for (int i = start + (width*t) / threads; i < start + (width*(t + 1)) / threads; i++)
410  {
411  for (int e = 0; e < Cube::EDGES; e++)
412  {
413  int& idx = eData[treeNodes[i]][e];
414  if (idx < 0)
415  {
416 #pragma omp critical
417  {
418  // found unindexed edge
419  ++paralellExceptionCount;
420  }
421  }
422  else
423  {
424  int div = idx / ( nodeCount*Cube::EDGES );
425  int rem = idx % ( nodeCount*Cube::EDGES );
426  idx = rem + offsets[div];
427  }
428  }
429  }
430  }
431  }
432 
433  if(paralellExceptionCount > 0)
434  POISSON_THROW_EXCEPTION (pcl::poisson::PoissonOpenMPException, "Found " << paralellExceptionCount << " unindexed edges during openMP loop execution.");
435 
436  }
437  int SortedTreeNodes::getMaxEdgeCount( const TreeOctNode* rootNode , int depth , int threads ) const
438  {
439  if( threads<=0 ) threads = 1;
440  int res = 1<<depth;
441  std::vector< std::vector< int > > edgeCount( threads );
442  for( int t=0 ; t<threads ; t++ ) edgeCount[t].resize( res*res*res , 0 );
443 
444 #pragma omp parallel for num_threads( threads )
445  for( int t=0 ; t<threads ; t++ )
446  {
447  std::vector< int >& _edgeCount = edgeCount[t];
448  TreeOctNode::ConstNeighborKey3 neighborKey;
449  neighborKey.set( maxDepth-1 );
450  int start = nodeCount[depth] , end = nodeCount[maxDepth] , range = end-start;
451  for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ )
452  {
453  TreeOctNode* node = treeNodes[start+i];
454  const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , depth );
455  int d , off[3];
456  node->depthAndOffset( d , off );
457 
458  for( int e=0 ; e<Cube::EDGES ; e++ )
459  {
460  bool edgeOwner = true;
461  int o , i , j;
462  Cube::FactorEdgeIndex( e , o , i , j );
464  for( int cc=0 ; cc<Square::CORNERS ; cc++ )
465  {
466  int ii , jj , x , y , z;
467  Square::FactorCornerIndex( cc , ii , jj );
468  ii += i , jj += j;
469  switch( o )
470  {
471  case 0: y = ii , z = jj , x = 1 ; break;
472  case 1: x = ii , z = jj , y = 1 ; break;
473  case 2: x = ii , y = jj , z = 1 ; break;
474  }
475  if( neighbors.neighbors[x][y][z] && cc<ac ) { edgeOwner = false ; break; }
476  }
477  if( edgeOwner ) _edgeCount[ ( ( off[0]>>(d-depth) ) * res * res) + ( ( off[1]>>(d-depth) ) * res) + ( off[2]>>(d-depth) ) ]++;
478  }
479  }
480  }
481  int maxCount = 0;
482  for( int i=0 ; i<res*res*res ; i++ )
483  {
484  int c = 0;
485  for( int t=0 ; t<threads ; t++ ) c += edgeCount[t][i];
486  maxCount = std::max< int >( maxCount , c );
487  }
488  return maxCount;
489  }
490 
491 
492 
493  //////////////////
494  // TreeNodeData //
495  //////////////////
498  {
499  if( UseIndex )
500  {
501  nodeIndex = -1;
502  centerWeightContribution=0;
503  }
504  else mcIndex=0;
505  normalIndex = -1;
506  constraint = solution = 0;
507  pointIndex = -1;
508  }
510 
511 
512  ////////////
513  // Octree //
514  ////////////
515  template<int Degree>
517 
518 
519 
520  template<int Degree>
522  double mem = 0;//double( MemoryInfo::Usage() ) / (1<<20);
523  if(mem>maxMemoryUsage){maxMemoryUsage=mem;}
524  return mem;
525  }
526 
527  template<int Degree>
529  {
530  threads = 1;
531  radius = 0;
532  width = 0;
533  postNormalSmooth = 0;
534  _constrainValues = false;
535  }
536 
537  template<int Degree>
538  int Octree<Degree>::NonLinearSplatOrientedPoint( TreeOctNode* node , const Point3D<Real>& position , const Point3D<Real>& normal )
539  {
540  double x , dxdy , dxdydz , dx[DIMENSION][SPLAT_ORDER+1];
541  int off[3];
542  TreeOctNode::Neighbors3& neighbors = neighborKey.setNeighbors( node );
543  double width;
544  Point3D<Real> center;
545  Real w;
546  node->centerAndWidth( center , w );
547  width=w;
548  for( int i=0 ; i<3 ; i++ )
549  {
550 #if SPLAT_ORDER==2
551  off[i] = 0;
552  x = ( center[i] - position[i] - width ) / width;
553  dx[i][0] = 1.125+1.500*x+0.500*x*x;
554  x = ( center[i] - position[i] ) / width;
555  dx[i][1] = 0.750 - x*x;
556 
557  dx[i][2] = 1. - dx[i][1] - dx[i][0];
558 #elif SPLAT_ORDER==1
559  x = ( position[i] - center[i] ) / width;
560  if( x<0 )
561  {
562  off[i] = 0;
563  dx[i][0] = -x;
564  }
565  else
566  {
567  off[i] = 1;
568  dx[i][0] = 1. - x;
569  }
570  dx[i][1] = 1. - dx[i][0];
571 #elif SPLAT_ORDER==0
572  off[i] = 1;
573  dx[i][0] = 1.;
574 #else
575 # error Splat order not supported
576 #endif // SPLAT_ORDER
577  }
578  for( int i=off[0] ; i<=off[0]+SPLAT_ORDER ; i++ ) for( int j=off[1] ; j<=off[1]+SPLAT_ORDER ; j++ )
579  {
580  dxdy = dx[0][i] * dx[1][j];
581  for( int k=off[2] ; k<=off[2]+SPLAT_ORDER ; k++ )
582  if( neighbors.neighbors[i][j][k] )
583  {
584  dxdydz = dxdy * dx[2][k];
585  TreeOctNode* _node = neighbors.neighbors[i][j][k];
586  int idx =_node->nodeData.normalIndex;
587  if( idx<0 )
588  {
589  Point3D<Real> n;
590  n[0] = n[1] = n[2] = 0;
591  _node->nodeData.nodeIndex = 0;
592  idx = _node->nodeData.normalIndex = int(normals->size());
593  normals->push_back(n);
594  }
595  (*normals)[idx] += normal * Real( dxdydz );
596  }
597  }
598  return 0;
599  }
600  template<int Degree>
601  Real Octree<Degree>::NonLinearSplatOrientedPoint( const Point3D<Real>& position , const Point3D<Real>& normal , int splatDepth , Real samplesPerNode ,
602  int minDepth , int maxDepth )
603  {
604  double dx;
605  Point3D<Real> n;
606  TreeOctNode* temp;
607  int cnt=0;
608  double width;
609  Point3D< Real > myCenter;
610  Real myWidth;
611  myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5);
612  myWidth = Real(1.0);
613 
614  temp = &tree;
615  while( temp->depth()<splatDepth )
616  {
617  if( !temp->children )
618  {
619  fprintf( stderr , "Octree<Degree>::NonLinearSplatOrientedPoint error\n" );
620  return -1;
621  }
622  int cIndex=TreeOctNode::CornerIndex(myCenter,position);
623  temp=&temp->children[cIndex];
624  myWidth/=2;
625  if(cIndex&1) myCenter[0] += myWidth/2;
626  else myCenter[0] -= myWidth/2;
627  if(cIndex&2) myCenter[1] += myWidth/2;
628  else myCenter[1] -= myWidth/2;
629  if(cIndex&4) myCenter[2] += myWidth/2;
630  else myCenter[2] -= myWidth/2;
631  }
632  Real alpha,newDepth;
633  NonLinearGetSampleDepthAndWeight( temp , position , samplesPerNode , newDepth , alpha );
634 
635  if( newDepth<minDepth ) newDepth=Real(minDepth);
636  if( newDepth>maxDepth ) newDepth=Real(maxDepth);
637  int topDepth=int(ceil(newDepth));
638 
639  dx = 1.0-(topDepth-newDepth);
640  if( topDepth<=minDepth )
641  {
642  topDepth=minDepth;
643  dx=1;
644  }
645  else if( topDepth>maxDepth )
646  {
647  topDepth=maxDepth;
648  dx=1;
649  }
650  while( temp->depth()>topDepth ) temp=temp->parent;
651  while( temp->depth()<topDepth )
652  {
653  if(!temp->children) temp->initChildren();
654  int cIndex=TreeOctNode::CornerIndex(myCenter,position);
655  temp=&temp->children[cIndex];
656  myWidth/=2;
657  if(cIndex&1) myCenter[0] += myWidth/2;
658  else myCenter[0] -= myWidth/2;
659  if(cIndex&2) myCenter[1] += myWidth/2;
660  else myCenter[1] -= myWidth/2;
661  if(cIndex&4) myCenter[2] += myWidth/2;
662  else myCenter[2] -= myWidth/2;
663  }
664  width = 1.0 / ( 1<<temp->depth() );
665  n = normal * alpha / Real( pow( width , 3 ) ) * Real( dx );
666  NonLinearSplatOrientedPoint( temp , position , n );
667  if( fabs(1.0-dx) > EPSILON )
668  {
669  dx = Real(1.0-dx);
670  temp = temp->parent;
671  width = 1.0 / ( 1<<temp->depth() );
672 
673  n = normal * alpha / Real( pow( width , 3 ) ) * Real( dx );
674  NonLinearSplatOrientedPoint( temp , position , n );
675  }
676  return alpha;
677  }
678  template<int Degree>
679  void Octree<Degree>::NonLinearGetSampleDepthAndWeight(TreeOctNode* node,const Point3D<Real>& position,Real samplesPerNode,Real& depth,Real& weight){
680  TreeOctNode* temp=node;
681  weight = Real(1.0)/NonLinearGetSampleWeight(temp,position);
682  if( weight>=samplesPerNode ) depth=Real( temp->depth() + log( weight / samplesPerNode ) / log(double(1<<(DIMENSION-1))) );
683  else
684  {
685  Real oldAlpha,newAlpha;
686  oldAlpha=newAlpha=weight;
687  while( newAlpha<samplesPerNode && temp->parent )
688  {
689  temp=temp->parent;
690  oldAlpha=newAlpha;
691  newAlpha=Real(1.0)/NonLinearGetSampleWeight(temp,position);
692  }
693  depth = Real( temp->depth() + log( newAlpha / samplesPerNode ) / log( newAlpha / oldAlpha ) );
694  }
695  weight=Real(pow(double(1<<(DIMENSION-1)),-double(depth)));
696  }
697 
698  template<int Degree>
700  {
701  Real weight=0;
702  double x,dxdy,dx[DIMENSION][3];
703  TreeOctNode::Neighbors3& neighbors=neighborKey.setNeighbors( node );
704  double width;
705  Point3D<Real> center;
706  Real w;
707  node->centerAndWidth(center,w);
708  width=w;
709 
710  for( int i=0 ; i<DIMENSION ; i++ )
711  {
712  x = ( center[i] - position[i] - width ) / width;
713  dx[i][0] = 1.125 + 1.500*x + 0.500*x*x;
714  x = ( center[i] - position[i] ) / width;
715  dx[i][1] = 0.750 - x*x;
716 
717  dx[i][2] = 1.0 - dx[i][1] - dx[i][0];
718  }
719 
720  for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ )
721  {
722  dxdy = dx[0][i] * dx[1][j];
723  for( int k=0 ; k<3 ; k++ ) if( neighbors.neighbors[i][j][k] )
724  weight += Real( dxdy * dx[2][k] * neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution );
725  }
726  return Real( 1.0 / weight );
727  }
728 
729  template<int Degree>
731  {
732  TreeOctNode::Neighbors3& neighbors = neighborKey.setNeighbors( node );
733  double x,dxdy,dx[DIMENSION][3];
734  double width;
735  Point3D<Real> center;
736  Real w;
737  node->centerAndWidth( center , w );
738  width=w;
739  const double SAMPLE_SCALE = 1. / ( 0.125 * 0.125 + 0.75 * 0.75 + 0.125 * 0.125 );
740 
741  for( int i=0 ; i<DIMENSION ; i++ )
742  {
743  x = ( center[i] - position[i] - width ) / width;
744  dx[i][0] = 1.125 + 1.500*x + 0.500*x*x;
745  x = ( center[i] - position[i] ) / width;
746  dx[i][1] = 0.750 - x*x;
747  dx[i][2] = 1. - dx[i][1] - dx[i][0];
748  // Note that we are splatting along a co-dimension one manifold, so uniform point samples
749  // do not generate a unit sample weight.
750  dx[i][0] *= SAMPLE_SCALE;
751  }
752 
753  for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ )
754  {
755  dxdy = dx[0][i] * dx[1][j] * weight;
756  for( int k=0 ; k<3 ; k++ ) if( neighbors.neighbors[i][j][k] )
757  neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution += Real( dxdy * dx[2][k] );
758  }
759  return 0;
760  }
761 
762  template< int Degree > template<typename PointNT> int
763  Octree<Degree>::setTree(typename pcl::PointCloud<PointNT>::ConstPtr input_, int maxDepth , int minDepth ,
764  int kernelDepth , Real samplesPerNode , Real scaleFactor , Point3D<Real>& center , Real& scale ,
765  int useConfidence , Real constraintWeight , bool adaptiveWeights )
766  {
767  _minDepth = std::min< int >( std::max< int >( 0 , minDepth ) , maxDepth );
768  _constrainValues = (constraintWeight>0);
769  double pointWeightSum = 0;
770  Point3D<Real> min , max , position , normal , myCenter;
771  Real myWidth;
772  int i , cnt=0;
773  TreeOctNode* temp;
774  int splatDepth=0;
775 
776  TreeNodeData::UseIndex = 1;
777  neighborKey.set( maxDepth );
778  splatDepth = kernelDepth;
779  if( splatDepth<0 ) splatDepth = 0;
780 
781 
782  tree.setFullDepth( _minDepth );
783  // Read through once to get the center and scale
784  while (cnt != input_->size ())
785  {
786  Point3D< Real > p;
787  p[0] = input_->points[cnt].x;
788  p[1] = input_->points[cnt].y;
789  p[2] = input_->points[cnt].z;
790 
791  for (i = 0; i < DIMENSION; i++)
792  {
793  if( !cnt || p[i]<min[i] ) min[i] = p[i];
794  if( !cnt || p[i]>max[i] ) max[i] = p[i];
795  }
796  cnt++;
797  }
798 
799  scale = std::max< Real >( max[0]-min[0] , std::max< Real >( max[1]-min[1] , max[2]-min[2] ) );
800  center = ( max+min ) /2;
801 
802  scale *= scaleFactor;
803  for( i=0 ; i<DIMENSION ; i++ ) center[i] -= scale/2;
804  if( splatDepth>0 )
805  {
806  cnt = 0;
807  while (cnt != input_->size ())
808  {
809  position[0] = input_->points[cnt].x;
810  position[1] = input_->points[cnt].y;
811  position[2] = input_->points[cnt].z;
812  normal[0] = input_->points[cnt].normal_x;
813  normal[1] = input_->points[cnt].normal_y;
814  normal[2] = input_->points[cnt].normal_z;
815 
816  for( i=0 ; i<DIMENSION ; i++ ) position[i] = ( position[i]-center[i] ) / scale;
817  myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5);
818  myWidth = Real(1.0);
819  for( i=0 ; i<DIMENSION ; i++ ) if( position[i]<myCenter[i]-myWidth/2 || position[i]>myCenter[i]+myWidth/2 ) break;
820  if( i!=DIMENSION ) continue;
821  Real weight=Real( 1. );
822  if( useConfidence ) weight = Real( Length(normal) );
823  temp = &tree;
824  int d=0;
825  while( d<splatDepth )
826  {
827  NonLinearUpdateWeightContribution( temp , position , weight );
828  if( !temp->children ) temp->initChildren();
829  int cIndex=TreeOctNode::CornerIndex(myCenter,position);
830  temp=&temp->children[cIndex];
831  myWidth/=2;
832  if(cIndex&1) myCenter[0] += myWidth/2;
833  else myCenter[0] -= myWidth/2;
834  if(cIndex&2) myCenter[1] += myWidth/2;
835  else myCenter[1] -= myWidth/2;
836  if(cIndex&4) myCenter[2] += myWidth/2;
837  else myCenter[2] -= myWidth/2;
838  d++;
839  }
840  NonLinearUpdateWeightContribution( temp , position , weight );
841  cnt++;
842  }
843  }
844 
845  normals = new std::vector< Point3D<Real> >();
846  cnt=0;
847  while (cnt != input_->size ())
848  {
849  position[0] = input_->points[cnt].x;
850  position[1] = input_->points[cnt].y;
851  position[2] = input_->points[cnt].z;
852  normal[0] = input_->points[cnt].normal_x;
853  normal[1] = input_->points[cnt].normal_y;
854  normal[2] = input_->points[cnt].normal_z;
855  cnt ++;
856  for( i=0 ; i<DIMENSION ; i++ ) position[i] = ( position[i]-center[i] ) / scale;
857  myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5);
858  myWidth = Real(1.0);
859  for( i=0 ; i<DIMENSION ; i++ ) if(position[i]<myCenter[i]-myWidth/2 || position[i]>myCenter[i]+myWidth/2) break;
860  if( i!=DIMENSION ) continue;
861  Real l = Real( Length( normal ) );
862  if( l!=l || l<=EPSILON ) continue;
863  if( !useConfidence ) normal /= l;
864 
865  l = Real(1.);
866  Real pointWeight = Real(1.f);
867  if( samplesPerNode>0 && splatDepth )
868  {
869  pointWeight = NonLinearSplatOrientedPoint( position , normal , splatDepth , samplesPerNode , _minDepth , maxDepth );
870  }
871  else
872  {
873  Real alpha=1;
874  temp = &tree;
875  int d=0;
876  if( splatDepth )
877  {
878  while( d<splatDepth )
879  {
880  int cIndex=TreeOctNode::CornerIndex(myCenter,position);
881  temp=&temp->children[cIndex];
882  myWidth/=2;
883  if(cIndex&1) myCenter[0]+=myWidth/2;
884  else myCenter[0]-=myWidth/2;
885  if(cIndex&2) myCenter[1]+=myWidth/2;
886  else myCenter[1]-=myWidth/2;
887  if(cIndex&4) myCenter[2]+=myWidth/2;
888  else myCenter[2]-=myWidth/2;
889  d++;
890  }
891  alpha = NonLinearGetSampleWeight( temp , position );
892  }
893  for( i=0 ; i<DIMENSION ; i++ ) normal[i]*=alpha;
894  while( d<maxDepth )
895  {
896  if(!temp->children){temp->initChildren();}
897  int cIndex=TreeOctNode::CornerIndex(myCenter,position);
898  temp=&temp->children[cIndex];
899  myWidth/=2;
900  if(cIndex&1) myCenter[0]+=myWidth/2;
901  else myCenter[0]-=myWidth/2;
902  if(cIndex&2) myCenter[1]+=myWidth/2;
903  else myCenter[1]-=myWidth/2;
904  if(cIndex&4) myCenter[2]+=myWidth/2;
905  else myCenter[2]-=myWidth/2;
906  d++;
907  }
908  NonLinearSplatOrientedPoint( temp , position , normal );
909  pointWeight = alpha;
910  }
911  pointWeight = 1;
912  pointWeightSum += pointWeight;
913  if( _constrainValues )
914  {
915  int d = 0;
916  TreeOctNode* temp = &tree;
917  myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5);
918  myWidth = Real(1.0);
919  while( 1 )
920  {
921  int idx = temp->nodeData.pointIndex;
922  if( idx==-1 )
923  {
924  Point3D< Real > p;
925  p[0] = p[1] = p[2] = 0;
926  idx = int( _points.size() );
927  _points.push_back( PointData( position*pointWeight , pointWeight ) );
928  temp->nodeData.pointIndex = idx;
929  }
930  else
931  {
932  _points[idx].weight += pointWeight;
933  _points[idx].position += position * pointWeight;
934  }
935 
936  int cIndex = TreeOctNode::CornerIndex( myCenter , position );
937  if( !temp->children ) break;
938  temp = &temp->children[cIndex];
939  myWidth /= 2;
940  if( cIndex&1 ) myCenter[0] += myWidth/2;
941  else myCenter[0] -= myWidth/2;
942  if( cIndex&2 ) myCenter[1] += myWidth/2;
943  else myCenter[1] -= myWidth/2;
944  if( cIndex&4 ) myCenter[2] += myWidth/2;
945  else myCenter[2] -= myWidth/2;
946  d++;
947  }
948  }
949  }
950 
951 
952  if( _constrainValues )
953  for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode(n) )
954  if( n->nodeData.pointIndex!=-1 )
955  {
956  int idx = n->nodeData.pointIndex;
957  _points[idx].position /= _points[idx].weight;
958  if( adaptiveWeights ) _points[idx].weight *= (1<<n->d);
959  else _points[idx].weight *= (1<<maxDepth);
960  _points[idx].weight *= Real( constraintWeight / pointWeightSum );
961  }
962 #if FORCE_NEUMANN_FIELD
963  for( TreeOctNode* node=tree.nextNode() ; node ; node=tree.nextNode( node ) )
964  {
965  int d , off[3] , res;
966  node->depthAndOffset( d , off );
967  res = 1<<d;
968  if( node->nodeData.normalIndex<0 ) continue;
969  Point3D< Real >& normal = (*normals)[node->nodeData.normalIndex];
970  for( int d=0 ; d<3 ; d++ ) if( off[d]==0 || off[d]==res-1 ) normal[d] = 0;
971  }
972 #endif // FORCE_NEUMANN_FIELD
973  _sNodes.set( tree );
974 
975 
976  return cnt;
977  }
978 
979 
980  template<int Degree>
981  void Octree<Degree>::setBSplineData( int maxDepth , Real normalSmooth , bool reflectBoundary )
982  {
983  radius = 0.5 + 0.5 * Degree;
984  width=int(double(radius+0.5-EPSILON)*2);
985  if( normalSmooth>0 ) postNormalSmooth = normalSmooth;
986  fData.set( maxDepth , true , reflectBoundary );
987  }
988 
989  template<int Degree>
991  {
992  int maxDepth = tree.maxDepth( );
993  TreeOctNode::NeighborKey5 nKey;
994  nKey.set( maxDepth );
995  for( int d=maxDepth ; d>0 ; d-- )
996  for( TreeOctNode* node=tree.nextNode() ; node ; node=tree.nextNode( node ) )
997  if( node->d==d )
998  {
999  int xStart=0 , xEnd=5 , yStart=0 , yEnd=5 , zStart=0 , zEnd=5;
1000  int c = int( node - node->parent->children );
1001  int x , y , z;
1002  Cube::FactorCornerIndex( c , x , y , z );
1003  if( x ) xStart = 1;
1004  else xEnd = 4;
1005  if( y ) yStart = 1;
1006  else yEnd = 4;
1007  if( z ) zStart = 1;
1008  else zEnd = 4;
1009  nKey.setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1010  }
1011  _sNodes.set( tree );
1012  MemoryUsage();
1013  }
1014  template< int Degree >
1015  Real Octree< Degree >::GetValue( const PointInfo points[3][3][3] , const bool hasPoints[3][3] , const int d[3] ) const
1016  {
1017  double v = 0.;
1018  const int min[] = { std::max<int>( 0 , d[0]+0 ) , std::max<int>( 0 , d[1]+0 ) , std::max<int>( 0 , d[2]+0 ) };
1019  const int max[] = { std::min<int>( 2 , d[0]+2 ) , std::min<int>( 2 , d[1]+2 ) , std::min<int>( 2 , d[2]+2 ) };
1020  for( int i=min[0] ; i<=max[0] ; i++ ) for( int j=min[1] ; j<=max[1] ; j++ )
1021  {
1022  if( !hasPoints[i][j] ) continue;
1023  const PointInfo* pInfo = points[i][j];
1024  int ii = -d[0]+i;
1025  int jj = -d[1]+j;
1026  for( int k=min[2] ; k<=max[2] ; k++ )
1027  if( pInfo[k].weightedValue )
1028  v += pInfo[k].splineValues[0][ii] * pInfo[k].splineValues[1][jj] * pInfo[k].splineValues[2][-d[2]+k];
1029  }
1030  return Real( v );
1031  }
1032  template<int Degree>
1033  Real Octree<Degree>::GetLaplacian( const int idx[DIMENSION] ) const
1034  {
1035  return Real( fData.vvDotTable[idx[0]] * fData.vvDotTable[idx[1]] * fData.vvDotTable[idx[2]] * (fData.ddDotTable[idx[0]]+fData.ddDotTable[idx[1]]+fData.ddDotTable[idx[2]] ) );
1036  }
1037  template< int Degree >
1038  Real Octree< Degree >::GetLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 ) const
1039  {
1040  int symIndex[] =
1041  {
1042  BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) ,
1043  BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) ,
1044  BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] )
1045  };
1046  return GetLaplacian( symIndex );
1047  }
1048  template< int Degree >
1049  Real Octree< Degree >::GetDivergence( const TreeOctNode* node1 , const TreeOctNode* node2 , const Point3D< Real >& normal1 ) const
1050  {
1051  int symIndex[] =
1052  {
1053  BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) ,
1054  BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) ,
1055  BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] ) ,
1056  };
1057  int aSymIndex[] =
1058  {
1059  #if GRADIENT_DOMAIN_SOLUTION
1060  // Take the dot-product of the vector-field with the gradient of the basis function
1061  fData.Index( node2->off[0] , node1->off[0] ) ,
1062  fData.Index( node2->off[1] , node1->off[1] ) ,
1063  fData.Index( node2->off[2] , node1->off[2] )
1064  #else // !GRADIENT_DOMAIN_SOLUTION
1065  // Take the dot-product of the divergence of the vector-field with the basis function
1066  fData.Index( node1->off[0] , node2->off[0] ) ,
1067  fData.Index( node1->off[1] , node2->off[1] ) ,
1068  fData.Index( node1->off[2] , node2->off[2] )
1069  #endif // GRADIENT_DOMAIN_SOLUTION
1070  };
1071  double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
1072 #if GRADIENT_DOMAIN_SOLUTION
1073  return Real( dot * ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) );
1074 #else // !GRADIENT_DOMAIN_SOLUTION
1075  return -Real( dot * ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) );
1076 #endif // GRADIENT_DOMAIN_SOLUTION
1077  }
1078  template< int Degree >
1079  Real Octree< Degree >::GetDivergenceMinusLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 , Real value1 , const Point3D<Real>& normal1 ) const
1080  {
1081  int symIndex[] =
1082  {
1083  BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) ,
1084  BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) ,
1085  BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] )
1086  };
1087  int aSymIndex[] =
1088  {
1089  #if GRADIENT_DOMAIN_SOLUTION
1090  // Take the dot-product of the vector-field with the gradient of the basis function
1091  fData.Index( node2->off[0] , node1->off[0] ) ,
1092  fData.Index( node2->off[1] , node1->off[1] ) ,
1093  fData.Index( node2->off[2] , node1->off[2] )
1094  #else // !GRADIENT_DOMAIN_SOLUTION
1095  // Take the dot-product of the divergence of the vector-field with the basis function
1096  fData.Index( node1->off[0] , node2->off[0] ) ,
1097  fData.Index( node1->off[1] , node2->off[1] ) ,
1098  fData.Index( node1->off[2] , node2->off[2] )
1099  #endif // GRADIENT_DOMAIN_SOLUTION
1100  };
1101  double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
1102  return Real( dot *
1103  (
1104  #if GRADIENT_DOMAIN_SOLUTION
1105  - ( fData.ddDotTable[ symIndex[0]] + fData.ddDotTable[ symIndex[1]] + fData.ddDotTable[ symIndex[2]] ) * value1
1106  + ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] )
1107  #else // !GRADIENT_DOMAIN_SOLUTION
1108  - ( fData.ddDotTable[ symIndex[0]] + fData.ddDotTable[ symIndex[1]] + fData.ddDotTable[ symIndex[2]] ) * value1
1109  - ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] )
1110  #endif // GRADIENT_DOMAIN_SOLUTION
1111  )
1112  );
1113  }
1114  template< int Degree >
1115  void Octree< Degree >::SetMatrixRowBounds( const TreeOctNode* node , int rDepth , const int rOff[3] , int& xStart , int& xEnd , int& yStart , int& yEnd , int& zStart , int& zEnd ) const
1116  {
1117  xStart = 0 , yStart = 0 , zStart = 0 , xEnd = 5 , yEnd = 5 , zEnd = 5;
1118  int depth , off[3];
1119  node->depthAndOffset( depth , off );
1120  int width = 1 << ( depth-rDepth );
1121  off[0] -= rOff[0] << ( depth-rDepth ) , off[1] -= rOff[1] << ( depth-rDepth ) , off[2] -= rOff[2] << ( depth-rDepth );
1122  if( off[0]<0 ) xStart = -off[0];
1123  if( off[1]<0 ) yStart = -off[1];
1124  if( off[2]<0 ) zStart = -off[2];
1125  if( off[0]>=width ) xEnd = 4 - ( off[0]-width );
1126  if( off[1]>=width ) yEnd = 4 - ( off[1]-width );
1127  if( off[2]>=width ) zEnd = 4 - ( off[2]-width );
1128  }
1129  template< int Degree >
1130  int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 ) const { return GetMatrixRowSize( neighbors5 , 0 , 5 , 0 , 5 , 0 , 5 ); }
1131  template< int Degree >
1132  int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const
1133  {
1134  int count = 0;
1135  for( int x=xStart ; x<=2 ; x++ )
1136  for( int y=yStart ; y<yEnd ; y++ )
1137  if( x==2 && y>2 ) continue;
1138  else for( int z=zStart ; z<zEnd ; z++ )
1139  if( x==2 && y==2 && z>2 ) continue;
1140  else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 )
1141  count++;
1142  return count;
1143  }
1144  template< int Degree >
1145  int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] ) const
1146  {
1147  return SetMatrixRow( neighbors5 , row , offset , stencil , 0 , 5 , 0 , 5 , 0 , 5 );
1148  }
1149  template< int Degree >
1150  int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const
1151  {
1152  bool hasPoints[3][3];
1153  Real diagonal = 0;
1154  PointInfo samples[3][3][3];
1155 
1156  int count = 0;
1157  const TreeOctNode* node = neighbors5.neighbors[2][2][2];
1158  int index[] = { int( node->off[0] ) , int( node->off[1] ), int( node->off[2] ) };
1159 
1160  if( _constrainValues )
1161  {
1162  int d , idx[3];
1163  node->depthAndOffset( d , idx );
1164  idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] );
1165  idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] );
1166  idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] );
1167  for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ )
1168  {
1169  hasPoints[j][k] = false;
1170  for( int l=0 ; l<3 ; l++ )
1171  {
1172  const TreeOctNode* _node = neighbors5.neighbors[j+1][k+1][l+1];
1173  if( _node && _node->nodeData.pointIndex!=-1 )
1174  {
1175  const PointData& pData = _points[ _node->nodeData.pointIndex ];
1176  PointInfo& pointInfo = samples[j][k][l];
1177  Real weight = pData.weight;
1178  Point3D< Real > p = pData.position;
1179  for( int s=0 ; s<3 ; s++ )
1180  {
1181  pointInfo.splineValues[0][s] = float( fData.baseBSplines[ idx[0]+j-s][s]( p[0] ) );
1182  pointInfo.splineValues[1][s] = float( fData.baseBSplines[ idx[1]+k-s][s]( p[1] ) );
1183  pointInfo.splineValues[2][s] = float( fData.baseBSplines[ idx[2]+l-s][s]( p[2] ) );
1184  }
1185  float value = pointInfo.splineValues[0][j] * pointInfo.splineValues[1][k] * pointInfo.splineValues[2][l];
1186  diagonal += value * value * weight;
1187  pointInfo.weightedValue = value * weight;
1188  for( int s=0 ; s<3 ; s++ ) pointInfo.splineValues[0][s] *= pointInfo.weightedValue;
1189  hasPoints[j][k] = true;
1190  }
1191  else samples[j][k][l].weightedValue = 0;
1192  }
1193  }
1194  }
1195 
1196  bool isInterior;
1197  int d , off[3];
1198  neighbors5.neighbors[2][2][2]->depthAndOffset( d , off );
1199  int mn = 2 , mx = (1<<d)-2;
1200  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
1201  for( int x=xStart ; x<=2 ; x++ )
1202  for( int y=yStart ; y<yEnd ; y++ )
1203  if( x==2 && y>2 ) continue;
1204  else for( int z=zStart ; z<zEnd ; z++ )
1205  if( x==2 && y==2 && z>2 ) continue;
1206  else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 )
1207  {
1208  const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
1209  int _index[] = { int( _node->off[0] ) , int( _node->off[1] ), int( _node->off[2] ) };
1210  Real temp;
1211  if( isInterior ) temp = Real( stencil[x][y][z] );
1212  else temp = GetLaplacian( node , _node );
1213  if( _constrainValues )
1214  {
1215  int _d[] = { _index[0]-index[0] , _index[1]-index[1] , _index[2]-index[2] };
1216  if( x==2 && y==2 && z==2 ) temp += diagonal;
1217  else temp += GetValue( samples , hasPoints , _d );
1218  }
1219  if( x==2 && y==2 && z==2 ) temp /= 2;
1220  if( fabs(temp)>MATRIX_ENTRY_EPSILON )
1221  {
1222  row[count].N = _node->nodeData.nodeIndex-offset;
1223  row[count].Value = temp;
1224  count++;
1225  }
1226  }
1227  return count;
1228  }
1229  template< int Degree >
1230  void Octree< Degree >::SetDivergenceStencil( int depth , Point3D< double > *stencil , bool scatter ) const
1231  {
1232  int offset[] = { 2 , 2 , 2 };
1233  short d , off[3];
1234  TreeOctNode::Index( depth , offset , d , off );
1235  int index1[3] , index2[3];
1236  if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
1237  else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
1238  for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
1239  {
1240  int _offset[] = { x , y , z };
1241  TreeOctNode::Index( depth , _offset , d , off );
1242  if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
1243  else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
1244  int symIndex[] =
1245  {
1246  BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) ,
1247  BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) ,
1248  BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) ,
1249  };
1250  int aSymIndex[] =
1251  {
1252  #if GRADIENT_DOMAIN_SOLUTION
1253  // Take the dot-product of the vector-field with the gradient of the basis function
1254  fData.Index( index1[0] , index2[0] ) ,
1255  fData.Index( index1[1] , index2[1] ) ,
1256  fData.Index( index1[2] , index2[2] )
1257  #else // !GRADIENT_DOMAIN_SOLUTION
1258  // Take the dot-product of the divergence of the vector-field with the basis function
1259  fData.Index( index2[0] , index1[0] ) ,
1260  fData.Index( index2[1] , index1[1] ) ,
1261  fData.Index( index2[2] , index1[2] )
1262  #endif // GRADIENT_DOMAIN_SOLUTION
1263  };
1264 
1265  double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
1266 #if GRADIENT_DOMAIN_SOLUTION
1267  Point3D<double> temp;
1268  temp[0] = fData.dvDotTable[aSymIndex[0]] * dot;
1269  temp[1] = fData.dvDotTable[aSymIndex[1]] * dot;
1270  temp[2] = fData.dvDotTable[aSymIndex[2]] * dot;
1271  stencil[25*x + 5*y + z] = temp;
1272  // stencil[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot;
1273  // stencil[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot;
1274  // stencil[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot;
1275 #else // !GRADIENT_DOMAIN_SOLUTION
1276  Point3D<double> temp;
1277  temp[0] = -fData.dvDotTable[aSymIndex[0]] * dot;
1278  temp[1] = -fData.dvDotTable[aSymIndex[1]] * dot;
1279  temp[2] = -fData.dvDotTable[aSymIndex[2]] * dot;
1280  stencil[25*x + 5*y + z] = temp;
1281  // stencil[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot;
1282  // stencil[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot;
1283  // stencil[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot;
1284 #endif // GRADIENT_DOMAIN_SOLUTION
1285  }
1286  }
1287  template< int Degree >
1288  void Octree< Degree >::SetLaplacianStencil( int depth , double stencil[5][5][5] ) const
1289  {
1290  int offset[] = { 2 , 2 , 2 };
1291  short d , off[3];
1292  TreeOctNode::Index( depth , offset , d , off );
1293  int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) };
1294  for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
1295  {
1296  int _offset[] = { x , y , z };
1297  short _d , _off[3];
1298  TreeOctNode::Index( depth , _offset , _d , _off );
1299  int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) };
1300  int symIndex[3];
1301  symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] );
1302  symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] );
1303  symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] );
1304  stencil[x][y][z] = GetLaplacian( symIndex );
1305  }
1306  }
1307  template< int Degree >
1308  void Octree< Degree >::SetLaplacianStencils( int depth , Stencil< double , 5 > stencils[2][2][2] ) const
1309  {
1310  if( depth<=1 ) return;
1311  for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
1312  {
1313  short d , off[3];
1314  int offset[] = { 4+i , 4+j , 4+k };
1315  TreeOctNode::Index( depth , offset , d , off );
1316  int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) };
1317  for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
1318  {
1319  int _offset[] = { x , y , z };
1320  short _d , _off[3];
1321  TreeOctNode::Index( depth-1 , _offset , _d , _off );
1322  int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) };
1323  int symIndex[3];
1324  symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] );
1325  symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] );
1326  symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] );
1327  stencils[i][j][k].values[x][y][z] = GetLaplacian( symIndex );
1328  }
1329  }
1330  }
1331  template< int Degree >
1332  void Octree< Degree >::SetDivergenceStencils( int depth , Stencil< Point3D< double > , 5 > stencils[2][2][2] , bool scatter ) const
1333  {
1334  if( depth<=1 ) return;
1335  int index1[3] , index2[3];
1336  for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
1337  {
1338  short d , off[3];
1339  int offset[] = { 4+i , 4+j , 4+k };
1340  TreeOctNode::Index( depth , offset , d , off );
1341  if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
1342  else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
1343  for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
1344  {
1345  int _offset[] = { x , y , z };
1346  TreeOctNode::Index( depth-1 , _offset , d , off );
1347  if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
1348  else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
1349 
1350  int symIndex[] =
1351  {
1352  BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) ,
1353  BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) ,
1354  BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) ,
1355  };
1356  int aSymIndex[] =
1357  {
1358  #if GRADIENT_DOMAIN_SOLUTION
1359  // Take the dot-product of the vector-field with the gradient of the basis function
1360  fData.Index( index1[0] , index2[0] ) ,
1361  fData.Index( index1[1] , index2[1] ) ,
1362  fData.Index( index1[2] , index2[2] )
1363  #else // !GRADIENT_DOMAIN_SOLUTION
1364  // Take the dot-product of the divergence of the vector-field with the basis function
1365  fData.Index( index2[0] , index1[0] ) ,
1366  fData.Index( index2[1] , index1[1] ) ,
1367  fData.Index( index2[2] , index1[2] )
1368  #endif // GRADIENT_DOMAIN_SOLUTION
1369  };
1370  double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
1371 #if GRADIENT_DOMAIN_SOLUTION
1372  stencils[i][j][k].values[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot;
1373  stencils[i][j][k].values[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot;
1374  stencils[i][j][k].values[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot;
1375 #else // !GRADIENT_DOMAIN_SOLUTION
1376  stencils[i][j][k].values[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot;
1377  stencils[i][j][k].values[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot;
1378  stencils[i][j][k].values[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot;
1379 #endif // GRADIENT_DOMAIN_SOLUTION
1380  }
1381  }
1382  }
1383  template< int Degree >
1384  void Octree< Degree >::SetEvaluationStencils( int depth , Stencil< double , 3 > stencil1[8] , Stencil< double , 3 > stencil2[8][8] ) const
1385  {
1386  if( depth>2 )
1387  {
1388  int idx[3];
1389  int off[] = { 2 , 2 , 2 };
1390  for( int c=0 ; c<8 ; c++ )
1391  {
1392  VertexData::CornerIndex( depth , off , c , fData.depth , idx );
1393  idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount;
1394  for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ )
1395  {
1396  short _d , _off[3];
1397  int _offset[] = { x+1 , y+1 , z+1 };
1398  TreeOctNode::Index( depth , _offset , _d , _off );
1399  stencil1[c].values[x][y][z] = fData.valueTables[ idx[0]+int(_off[0]) ] * fData.valueTables[ idx[1]+int(_off[1]) ] * fData.valueTables[ idx[2]+int(_off[2]) ];
1400  }
1401  }
1402  }
1403  if( depth>3 )
1404  for( int _c=0 ; _c<8 ; _c++ )
1405  {
1406  int idx[3];
1407  int _cx , _cy , _cz;
1408  Cube::FactorCornerIndex( _c , _cx , _cy , _cz );
1409  int off[] = { 4+_cx , 4+_cy , 4+_cz };
1410  for( int c=0 ; c<8 ; c++ )
1411  {
1412  VertexData::CornerIndex( depth , off , c , fData.depth , idx );
1413  idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount;
1414  for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ )
1415  {
1416  short _d , _off[3];
1417  int _offset[] = { x+1 , y+1 , z+1 };
1418  TreeOctNode::Index( depth-1 , _offset , _d , _off );
1419  stencil2[_c][c].values[x][y][z] = fData.valueTables[ idx[0]+int(_off[0]) ] * fData.valueTables[ idx[1]+int(_off[1]) ] * fData.valueTables[ idx[2]+int(_off[2]) ];
1420  }
1421  }
1422  }
1423  }
1424  template< int Degree >
1425  void Octree< Degree >::UpdateCoarserSupportBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ )
1426  {
1427  if( node->parent )
1428  {
1429  int x , y , z , c = int( node - node->parent->children );
1430  Cube::FactorCornerIndex( c , x , y , z );
1431  if( x==0 ) endX = 4;
1432  else startX = 1;
1433  if( y==0 ) endY = 4;
1434  else startY = 1;
1435  if( z==0 ) endZ = 4;
1436  else startZ = 1;
1437  }
1438  }
1439 
1440  template< int Degree >
1441  void Octree< Degree >::UpdateConstraintsFromCoarser( const OctNode< TreeNodeData , Real >::NeighborKey5& neighborKey5 , TreeOctNode* node , Real* metSolution , const Stencil< double , 5 >& lapStencil ) const
1442  {
1443  bool isInterior;
1444  {
1445  int d , off[3];
1446  node->depthAndOffset( d , off );
1447  int mn = 4 , mx = (1<<d)-4;
1448  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
1449  }
1450  Real constraint = Real( 0 );
1451  int depth = node->depth();
1452  if( depth<=_minDepth ) return;
1453  int i = node->nodeData.nodeIndex;
1454  // Offset the constraints using the solution from lower resolutions.
1455  int startX = 0 , endX = 5 , startY = 0 , endY = 5 , startZ = 0 , endZ = 5;
1456  UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ );
1457 
1458  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth-1];
1459  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
1460  if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 )
1461  {
1462  const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
1463  Real _solution = metSolution[ _node->nodeData.nodeIndex ];
1464  {
1465  if( isInterior ) node->nodeData.constraint -= Real( lapStencil.values[x][y][z] * _solution );
1466  else node->nodeData.constraint -= GetLaplacian( _node , node ) * _solution;
1467  }
1468  }
1469  if( _constrainValues )
1470  {
1471  int d , idx[3];
1472  node->depthAndOffset( d, idx );
1473  idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] );
1474  idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] );
1475  idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] );
1476  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth];
1477  for( int x=1 ; x<4 ; x++ ) for( int y=1 ; y<4 ; y++ ) for( int z=1 ; z<4 ; z++ )
1478  if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.pointIndex!=-1 )
1479  {
1480  const PointData& pData = _points[ neighbors5.neighbors[x][y][z]->nodeData.pointIndex ];
1481  Real pointValue = pData.value;
1482  Point3D< Real > p = pData.position;
1483  node->nodeData.constraint -=
1484  Real(
1485  fData.baseBSplines[idx[0]][x-1]( p[0] ) *
1486  fData.baseBSplines[idx[1]][y-1]( p[1] ) *
1487  fData.baseBSplines[idx[2]][z-1]( p[2] ) *
1488  pointValue
1489  );
1490  }
1491  }
1492  }
1494  {
1495  int start;
1496  double v[2];
1497  UpSampleData( void ) { start = 0 , v[0] = v[1] = 0.; }
1498  UpSampleData( int s , double v1 , double v2 ) { start = s , v[0] = v1 , v[1] = v2; }
1499  };
1500  template< int Degree >
1501  void Octree< Degree >::UpSampleCoarserSolution( int depth , const SortedTreeNodes& sNodes , Vector< Real >& Solution ) const
1502  {
1503  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1504  Solution.Resize( range );
1505  if( !depth ) return;
1506  else if( depth==1 ) for( int i=start ; i<end ; i++ ) Solution[i-start] += sNodes.treeNodes[0]->nodeData.solution;
1507  else
1508  {
1509  // For every node at the current depth
1510 #pragma omp parallel for num_threads( threads )
1511  for( int t=0 ; t<threads ; t++ )
1512  {
1513  TreeOctNode::NeighborKey3 neighborKey;
1514  neighborKey.set( depth );
1515  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1516  {
1517  int d , off[3];
1518  UpSampleData usData[3];
1519  sNodes.treeNodes[i]->depthAndOffset( d , off );
1520  for( int d=0 ; d<3 ; d++ )
1521  {
1522  if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
1523  else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
1524  else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
1525  else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
1526  }
1527  neighborKey.getNeighbors( sNodes.treeNodes[i] );
1528  for( int ii=0 ; ii<2 ; ii++ )
1529  {
1530  int _ii = ii + usData[0].start;
1531  double dx = usData[0].v[ii];
1532  for( int jj=0 ; jj<2 ; jj++ )
1533  {
1534  int _jj = jj + usData[1].start;
1535  double dxy = dx * usData[1].v[jj];
1536  for( int kk=0 ; kk<2 ; kk++ )
1537  {
1538  int _kk = kk + usData[2].start;
1539  double dxyz = dxy * usData[2].v[kk];
1540  Solution[i-start] += Real( neighborKey.neighbors[depth-1].neighbors[_ii][_jj][_kk]->nodeData.solution * dxyz );
1541  }
1542  }
1543  }
1544  }
1545  }
1546  }
1547  // Clear the coarser solution
1548  start = sNodes.nodeCount[depth-1] , end = sNodes.nodeCount[depth] , range = end-start;
1549 #pragma omp parallel for num_threads( threads )
1550  for( int i=start ; i<end ; i++ ) sNodes.treeNodes[i]->nodeData.solution = Real( 0. );
1551  }
1552  template< int Degree >
1553  void Octree< Degree >::DownSampleFinerConstraints( int depth , SortedTreeNodes& sNodes ) const
1554  {
1555  if( !depth ) return;
1556 #pragma omp parallel for num_threads( threads )
1557  for( int i=sNodes.nodeCount[depth-1] ; i<sNodes.nodeCount[depth] ; i++ )
1558  sNodes.treeNodes[i]->nodeData.constraint = Real( 0 );
1559 
1560  if( depth==1 )
1561  {
1562  sNodes.treeNodes[0]->nodeData.constraint = Real( 0 );
1563  for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) sNodes.treeNodes[0]->nodeData.constraint += sNodes.treeNodes[i]->nodeData.constraint;
1564  return;
1565  }
1566  std::vector< Vector< double > > constraints( threads );
1567  for( int t=0 ; t<threads ; t++ ) constraints[t].Resize( sNodes.nodeCount[depth] - sNodes.nodeCount[depth-1] ) , constraints[t].SetZero();
1568  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1569  int lStart = sNodes.nodeCount[depth-1] , lEnd = sNodes.nodeCount[depth];
1570  // For every node at the current depth
1571 #pragma omp parallel for num_threads( threads )
1572  for( int t=0 ; t<threads ; t++ )
1573  {
1574  TreeOctNode::NeighborKey3 neighborKey;
1575  neighborKey.set( depth );
1576  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1577  {
1578  int d , off[3];
1579  UpSampleData usData[3];
1580  sNodes.treeNodes[i]->depthAndOffset( d , off );
1581  for( int d=0 ; d<3 ; d++ )
1582  {
1583  if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
1584  else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
1585  else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
1586  else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
1587  }
1588  neighborKey.getNeighbors( sNodes.treeNodes[i] );
1589  TreeOctNode::Neighbors3& neighbors = neighborKey.neighbors[depth-1];
1590  for( int ii=0 ; ii<2 ; ii++ )
1591  {
1592  int _ii = ii + usData[0].start;
1593  double dx = usData[0].v[ii];
1594  for( int jj=0 ; jj<2 ; jj++ )
1595  {
1596  int _jj = jj + usData[1].start;
1597  double dxy = dx * usData[1].v[jj];
1598  for( int kk=0 ; kk<2 ; kk++ )
1599  {
1600  int _kk = kk + usData[2].start;
1601  double dxyz = dxy * usData[2].v[kk];
1602  constraints[t][neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex-lStart] += sNodes.treeNodes[i]->nodeData.constraint * dxyz;
1603  }
1604  }
1605  }
1606  }
1607  }
1608 #pragma omp parallel for num_threads( threads )
1609  for( int i=lStart ; i<lEnd ; i++ )
1610  {
1611  Real cSum = Real(0.);
1612  for( int t=0 ; t<threads ; t++ ) cSum += constraints[t][i-lStart];
1613  sNodes.treeNodes[i]->nodeData.constraint += cSum;
1614  }
1615  }
1616  template< int Degree >
1617  template< class C >
1618  void Octree< Degree >::DownSample( int depth , const SortedTreeNodes& sNodes , C* constraints ) const
1619  {
1620  if( depth==0 ) return;
1621  if( depth==1 )
1622  {
1623  for( int i=sNodes.nodeCount[1] ; i<sNodes.nodeCount[2] ; i++ ) constraints[0] += constraints[i];
1624  return;
1625  }
1626  std::vector< Vector< C > > _constraints( threads );
1627  for( int t=0 ; t<threads ; t++ ) _constraints[t].Resize( sNodes.nodeCount[depth] - sNodes.nodeCount[depth-1] );
1628  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start , lStart = sNodes.nodeCount[depth-1] , lEnd = sNodes.nodeCount[depth];
1629  // For every node at the current depth
1630 #pragma omp parallel for num_threads( threads )
1631  for( int t=0 ; t<threads ; t++ )
1632  {
1633  TreeOctNode::NeighborKey3 neighborKey;
1634  neighborKey.set( depth );
1635  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1636  {
1637  int d , off[3];
1638  UpSampleData usData[3];
1639  sNodes.treeNodes[i]->depthAndOffset( d , off );
1640  for( int d=0 ; d<3 ; d++ )
1641  {
1642  if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
1643  else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
1644  else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
1645  else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
1646  }
1647  TreeOctNode::Neighbors3& neighbors = neighborKey.getNeighbors( sNodes.treeNodes[i]->parent );
1648  C c = constraints[i];
1649  for( int ii=0 ; ii<2 ; ii++ )
1650  {
1651  int _ii = ii + usData[0].start;
1652  C cx = C( c*usData[0].v[ii] );
1653  for( int jj=0 ; jj<2 ; jj++ )
1654  {
1655  int _jj = jj + usData[1].start;
1656  C cxy = C( cx*usData[1].v[jj] );
1657  for( int kk=0 ; kk<2 ; kk++ )
1658  {
1659  int _kk = kk + usData[2].start;
1660  if( neighbors.neighbors[_ii][_jj][_kk] )
1661  _constraints[t][neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex-lStart] += C( cxy*usData[2].v[kk] );
1662  }
1663  }
1664  }
1665  }
1666  }
1667 #pragma omp parallel for num_threads( threads )
1668  for( int i=lStart ; i<lEnd ; i++ )
1669  {
1670  C cSum = C(0);
1671  for( int t=0 ; t<threads ; t++ ) cSum += _constraints[t][i-lStart];
1672  constraints[i] += cSum;
1673  }
1674  }
1675  template< int Degree >
1676  template< class C >
1677  void Octree< Degree >::UpSample( int depth , const SortedTreeNodes& sNodes , C* coefficients ) const
1678  {
1679  if ( depth==0 ) return;
1680  else if( depth==1 )
1681  {
1682  for( int i=sNodes.nodeCount[1] ; i<sNodes.nodeCount[2] ; i++ ) coefficients[i] += coefficients[0];
1683  return;
1684  }
1685 
1686  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1687  // For every node at the current depth
1688 #pragma omp parallel for num_threads( threads )
1689  for( int t=0 ; t<threads ; t++ )
1690  {
1691  TreeOctNode::NeighborKey3 neighborKey;
1692  neighborKey.set( depth-1 );
1693  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1694  {
1695  TreeOctNode* node = sNodes.treeNodes[i];
1696  int d , off[3];
1697  UpSampleData usData[3];
1698  node->depthAndOffset( d , off );
1699  for( int d=0 ; d<3 ; d++ )
1700  {
1701  if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
1702  else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
1703  else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
1704  else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
1705  }
1706  TreeOctNode::Neighbors3& neighbors = neighborKey.getNeighbors( node->parent );
1707  for( int ii=0 ; ii<2 ; ii++ )
1708  {
1709  int _ii = ii + usData[0].start;
1710  double dx = usData[0].v[ii];
1711  for( int jj=0 ; jj<2 ; jj++ )
1712  {
1713  int _jj = jj + usData[1].start;
1714  double dxy = dx * usData[1].v[jj];
1715  for( int kk=0 ; kk<2 ; kk++ )
1716  {
1717  int _kk = kk + usData[2].start;
1718  if( neighbors.neighbors[_ii][_jj][_kk] )
1719  {
1720  double dxyz = dxy * usData[2].v[kk];
1721  int _i = neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex;
1722  coefficients[i] += coefficients[_i] * Real( dxyz );
1723  }
1724  }
1725  }
1726  }
1727  }
1728  }
1729  }
1730  template< int Degree >
1731  void Octree< Degree >::SetCoarserPointValues( int depth , const SortedTreeNodes& sNodes , Real* metSolution )
1732  {
1733  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1734  // For every node at the current depth
1735 #pragma omp parallel for num_threads( threads )
1736  for( int t=0 ; t<threads ; t++ )
1737  {
1738  TreeOctNode::NeighborKey3 neighborKey;
1739  neighborKey.set( depth );
1740  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1741  {
1742  int pIdx = sNodes.treeNodes[i]->nodeData.pointIndex;
1743  if( pIdx!=-1 )
1744  {
1745  neighborKey.getNeighbors( sNodes.treeNodes[i] );
1746  _points[ pIdx ].value = WeightedCoarserFunctionValue( neighborKey , sNodes.treeNodes[i] , metSolution );
1747  }
1748  }
1749  }
1750  }
1751  template< int Degree >
1752  Real Octree< Degree >::WeightedCoarserFunctionValue( const OctNode< TreeNodeData , Real >::NeighborKey3& neighborKey , const TreeOctNode* pointNode , Real* metSolution ) const
1753  {
1754  int depth = pointNode->depth();
1755  if( !depth || pointNode->nodeData.pointIndex==-1 ) return Real(0.);
1756  double pointValue = 0;
1757 
1758  Real weight = _points[ pointNode->nodeData.pointIndex ].weight;
1759  Point3D< Real > p = _points[ pointNode->nodeData.pointIndex ].position;
1760 
1761  // Iterate over all basis functions that overlap the point at the coarser resolutions
1762  {
1763  int d , _idx[3];
1764  const TreeOctNode::Neighbors3& neighbors = neighborKey.neighbors[depth-1];
1765  neighbors.neighbors[1][1][1]->depthAndOffset( d , _idx );
1766  _idx[0] = BinaryNode< double >::CenterIndex( d , _idx[0]-1 );
1767  _idx[1] = BinaryNode< double >::CenterIndex( d , _idx[1]-1 );
1768  _idx[2] = BinaryNode< double >::CenterIndex( d , _idx[2]-1 );
1769  for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) for( int l=0 ; l<3 ; l++ )
1770  if( neighbors.neighbors[j][k][l] && neighbors.neighbors[j][k][l]->nodeData.nodeIndex>=0 )
1771  {
1772  // Accumulate the contribution from these basis nodes
1773  const TreeOctNode* basisNode = neighbors.neighbors[j][k][l];
1774  int idx[] = { _idx[0]+j , _idx[1]+k , _idx[2]+l };
1775  pointValue +=
1776  fData.baseBSplines[ idx[0] ][2-j]( p[0] ) *
1777  fData.baseBSplines[ idx[1] ][2-k]( p[1] ) *
1778  fData.baseBSplines[ idx[2] ][2-l]( p[2] ) *
1779  metSolution[basisNode->nodeData.nodeIndex];
1780  }
1781  }
1782  return Real( pointValue * weight );
1783  }
1784  template<int Degree>
1785  int Octree<Degree>::GetFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix , int depth , const SortedTreeNodes& sNodes , Real* metSolution )
1786  {
1787  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1788  double stencil[5][5][5];
1789  SetLaplacianStencil( depth , stencil );
1790  Stencil< double , 5 > stencils[2][2][2];
1791  SetLaplacianStencils( depth , stencils );
1792  matrix.Resize( range );
1793 #pragma omp parallel for num_threads( threads )
1794  for( int t=0 ; t<threads ; t++ )
1795  {
1796  TreeOctNode::NeighborKey5 neighborKey5;
1797  neighborKey5.set( depth );
1798  for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ )
1799  {
1800  TreeOctNode* node = sNodes.treeNodes[i+start];
1801  neighborKey5.getNeighbors( node );
1802 
1803  // Get the matrix row size
1804  int count = GetMatrixRowSize( neighborKey5.neighbors[depth] );
1805 
1806  // Allocate memory for the row
1807 #pragma omp critical (matrix_set_row_size)
1808  {
1809  matrix.SetRowSize( i , count );
1810  }
1811 
1812  // Set the row entries
1813  matrix.rowSizes[i] = SetMatrixRow( neighborKey5.neighbors[depth] , matrix[i] , start , stencil );
1814 
1815  // Offset the constraints using the solution from lower resolutions.
1816  int x , y , z , c;
1817  if( node->parent )
1818  {
1819  c = int( node - node->parent->children );
1820  Cube::FactorCornerIndex( c , x , y , z );
1821  }
1822  else x = y = z = 0;
1823  UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] );
1824  }
1825  }
1826  return 1;
1827  }
1828  template<int Degree>
1829  int Octree<Degree>::GetRestrictedFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix,int depth,const int* entries,int entryCount,
1830  const TreeOctNode* rNode , Real radius ,
1831  const SortedTreeNodes& sNodes , Real* metSolution )
1832  {
1833  for( int i=0 ; i<entryCount ; i++ ) sNodes.treeNodes[ entries[i] ]->nodeData.nodeIndex = i;
1834  double stencil[5][5][5];
1835  int rDepth , rOff[3];
1836  rNode->depthAndOffset( rDepth , rOff );
1837  matrix.Resize( entryCount );
1838  SetLaplacianStencil( depth , stencil );
1839  Stencil< double , 5 > stencils[2][2][2];
1840  SetLaplacianStencils( depth , stencils );
1841 #pragma omp parallel for num_threads( threads )
1842  for( int t=0 ; t<threads ; t++ )
1843  {
1844  TreeOctNode::NeighborKey5 neighborKey5;
1845  neighborKey5.set( depth );
1846  for( int i=(entryCount*t)/threads ; i<(entryCount*(t+1))/threads ; i++ )
1847  {
1848  TreeOctNode* node = sNodes.treeNodes[ entries[i] ];
1849  int d , off[3];
1850  node->depthAndOffset( d , off );
1851  off[0] >>= (depth-rDepth) , off[1] >>= (depth-rDepth) , off[2] >>= (depth-rDepth);
1852  bool isInterior = ( off[0]==rOff[0] && off[1]==rOff[1] && off[2]==rOff[2] );
1853 
1854  neighborKey5.getNeighbors( node );
1855 
1856  int xStart=0 , xEnd=5 , yStart=0 , yEnd=5 , zStart=0 , zEnd=5;
1857  if( !isInterior ) SetMatrixRowBounds( neighborKey5.neighbors[depth].neighbors[2][2][2] , rDepth , rOff , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1858 
1859  // Get the matrix row size
1860  int count = GetMatrixRowSize( neighborKey5.neighbors[depth] , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1861 
1862  // Allocate memory for the row
1863 #pragma omp critical (matrix_set_row_size)
1864  {
1865  matrix.SetRowSize( i , count );
1866  }
1867 
1868  // Set the matrix row entries
1869  matrix.rowSizes[i] = SetMatrixRow( neighborKey5.neighbors[depth] , matrix[i] , 0 , stencil , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1870  // Adjust the system constraints
1871  int x , y , z , c;
1872  if( node->parent )
1873  {
1874  c = int( node - node->parent->children );
1875  Cube::FactorCornerIndex( c , x , y , z );
1876  }
1877  else x = y = z = 0;
1878  UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] );
1879  }
1880  }
1881  for( int i=0 ; i<entryCount ; i++ ) sNodes.treeNodes[entries[i]]->nodeData.nodeIndex = entries[i];
1882  return 1;
1883  }
1884 
1885  template<int Degree>
1886  int Octree<Degree>::LaplacianMatrixIteration( int subdivideDepth , bool showResidual , int minIters , double accuracy )
1887  {
1888  int i,iter=0;
1889  double t = 0;
1890  fData.setDotTables( fData.DD_DOT_FLAG | fData.DV_DOT_FLAG );
1891 
1892  SparseMatrix< float >::SetAllocator( MEMORY_ALLOCATOR_BLOCK_SIZE );
1893  _sNodes.treeNodes[0]->nodeData.solution = 0;
1894 
1895  std::vector< Real > metSolution( _sNodes.nodeCount[ _sNodes.maxDepth ] , 0 );
1896 
1897  for( i=1 ; i<_sNodes.maxDepth ; i++ )
1898  {
1899  if( subdivideDepth>0 ) iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , subdivideDepth , showResidual , minIters , accuracy );
1900  else iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , showResidual , minIters , accuracy );
1901  }
1903  fData.clearDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG | fData.DD_DOT_FLAG );
1904 
1905  return iter;
1906  }
1907 
1908  template<int Degree>
1909  int Octree<Degree>::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , bool showResidual , int minIters , double accuracy )
1910  {
1911  int iter = 0;
1912  Vector< Real > X , B;
1914  double systemTime=0. , solveTime=0. , updateTime=0. , evaluateTime = 0.;
1915 
1916  X.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] );
1917  if( depth<=_minDepth ) UpSampleCoarserSolution( depth , sNodes , X );
1918  else
1919  {
1920  // Up-sample the cumulative solution into the previous depth
1921  UpSample( depth-1 , sNodes , metSolution );
1922  // Add in the solution from that depth
1923  if( depth )
1924 #pragma omp parallel for num_threads( threads )
1925  for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution;
1926  }
1927  if( _constrainValues )
1928  {
1929  SetCoarserPointValues( depth , sNodes , metSolution );
1930  }
1931 
1933  {
1934  int maxECount = ( (2*Degree+1)*(2*Degree+1)*(2*Degree+1) + 1 ) / 2;
1935  maxECount = ( ( maxECount + 15 ) / 16 ) * 16;
1936  M.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] );
1937  for( int i=0 ; i<M.rows ; i++ ) M.SetRowSize( i , maxECount );
1938  }
1939 
1940  {
1941  // Get the system matrix
1943  GetFixedDepthLaplacian( M , depth , sNodes , metSolution );
1944  // Set the constraint vector
1945  B.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] );
1946  for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) B[i-sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.constraint;
1947  }
1948 
1949  // Solve the linear system
1950  iter += SparseSymmetricMatrix< Real >::Solve( M , B , std::max< int >( int( pow( M.rows , ITERATION_POWER ) ) , minIters ) , X , Real(accuracy) , 0 , threads , (depth<=_minDepth) && !_constrainValues );
1951 
1952  if( showResidual )
1953  {
1954  double mNorm = 0;
1955  for( int i=0 ; i<M.rows ; i++ ) for( int j=0 ; j<M.rowSizes[i] ; j++ ) mNorm += M[i][j].Value * M[i][j].Value;
1956  double bNorm = B.Norm( 2 ) , rNorm = ( B - M * X ).Norm( 2 );
1957  printf( "\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter );
1958  }
1959 
1960  // Copy the solution back into the tree (over-writing the constraints)
1961  for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) sNodes.treeNodes[i]->nodeData.solution = Real( X[i-sNodes.nodeCount[depth]] );
1962 
1963  return iter;
1964  }
1965  template<int Degree>
1966  int Octree<Degree>::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , int startingDepth , bool showResidual , int minIters , double accuracy )
1967  {
1968  if( startingDepth>=depth ) return SolveFixedDepthMatrix( depth , sNodes , metSolution , showResidual , minIters , accuracy );
1969 
1970  int i , j , d , tIter=0;
1972  Vector< Real > B , B_ , X_;
1973  AdjacencySetFunction asf;
1974  AdjacencyCountFunction acf;
1975  double systemTime = 0 , solveTime = 0 , memUsage = 0 , evaluateTime = 0 , gTime = 0, sTime = 0;
1976  Real myRadius , myRadius2;
1977 
1978  if( depth>_minDepth )
1979  {
1980  // Up-sample the cumulative solution into the previous depth
1981  UpSample( depth-1 , sNodes , metSolution );
1982  // Add in the solution from that depth
1983  if( depth )
1984 #pragma omp parallel for num_threads( threads )
1985  for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution;
1986  }
1987 
1988  if( _constrainValues )
1989  {
1990  SetCoarserPointValues( depth , sNodes , metSolution );
1991  }
1992  B.Resize( sNodes.nodeCount[depth+1] - sNodes.nodeCount[depth] );
1993 
1994  // Back-up the constraints
1995  for( i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ )
1996  {
1997  B[ i-sNodes.nodeCount[depth] ] = sNodes.treeNodes[i]->nodeData.constraint;
1998  sNodes.treeNodes[i]->nodeData.constraint = 0;
1999  }
2000 
2001  myRadius = 2*radius-Real(0.5);
2002  myRadius = int(myRadius-ROUND_EPS)+ROUND_EPS;
2003  myRadius2 = Real(radius+ROUND_EPS-0.5);
2004  d = depth-startingDepth;
2005  std::vector< int > subDimension( sNodes.nodeCount[d+1]-sNodes.nodeCount[d] );
2006  int maxDimension = 0;
2007  for( i=sNodes.nodeCount[d] ; i<sNodes.nodeCount[d+1] ; i++ )
2008  {
2009  // Count the number of nodes at depth "depth" that lie under sNodes.treeNodes[i]
2010  acf.adjacencyCount = 0;
2011  for( TreeOctNode* temp=sNodes.treeNodes[i]->nextNode() ; temp ; )
2012  {
2013  if( temp->depth()==depth ) acf.adjacencyCount++ , temp = sNodes.treeNodes[i]->nextBranch( temp );
2014  else temp = sNodes.treeNodes[i]->nextNode ( temp );
2015  }
2016  for( j=sNodes.nodeCount[d] ; j<sNodes.nodeCount[d+1] ; j++ )
2017  {
2018  if( i==j ) continue;
2019  TreeOctNode::ProcessFixedDepthNodeAdjacentNodes( fData.depth , sNodes.treeNodes[i] , 1 , sNodes.treeNodes[j] , 2*width-1 , depth , &acf );
2020  }
2021  subDimension[i-sNodes.nodeCount[d]] = acf.adjacencyCount;
2022  maxDimension = std::max< int >( maxDimension , subDimension[i-sNodes.nodeCount[d]] );
2023  }
2024  asf.adjacencies = new int[maxDimension];
2025  MapReduceVector< Real > mrVector;
2026  mrVector.resize( threads , maxDimension );
2027  // Iterate through the coarse-level nodes
2028  for( i=sNodes.nodeCount[d] ; i<sNodes.nodeCount[d+1] ; i++ )
2029  {
2030  int iter = 0;
2031  // Count the number of nodes at depth "depth" that lie under sNodes.treeNodes[i]
2032  acf.adjacencyCount = subDimension[i-sNodes.nodeCount[d]];
2033  if( !acf.adjacencyCount ) continue;
2034 
2035  // Set the indices for the nodes under, or near, sNodes.treeNodes[i].
2036  asf.adjacencyCount = 0;
2037  for( TreeOctNode* temp=sNodes.treeNodes[i]->nextNode() ; temp ; )
2038  {
2039  if( temp->depth()==depth ) asf.adjacencies[ asf.adjacencyCount++ ] = temp->nodeData.nodeIndex , temp = sNodes.treeNodes[i]->nextBranch( temp );
2040  else temp = sNodes.treeNodes[i]->nextNode ( temp );
2041  }
2042  for( j=sNodes.nodeCount[d] ; j<sNodes.nodeCount[d+1] ; j++ )
2043  {
2044  if( i==j ) continue;
2045  TreeOctNode::ProcessFixedDepthNodeAdjacentNodes( fData.depth , sNodes.treeNodes[i] , 1 , sNodes.treeNodes[j] , 2*width-1 , depth , &asf );
2046  }
2047 
2048  // Get the associated constraint vector
2049  B_.Resize( asf.adjacencyCount );
2050  for( j=0 ; j<asf.adjacencyCount ; j++ ) B_[j] = B[ asf.adjacencies[j]-sNodes.nodeCount[depth] ];
2051 
2052  X_.Resize( asf.adjacencyCount );
2053 #pragma omp parallel for num_threads( threads ) schedule( static )
2054  for( j=0 ; j<asf.adjacencyCount ; j++ )
2055  {
2056  X_[j] = sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution;
2057  }
2058  // Get the associated matrix
2060  GetRestrictedFixedDepthLaplacian( _M , depth , asf.adjacencies , asf.adjacencyCount , sNodes.treeNodes[i] , myRadius , sNodes , metSolution );
2061 #pragma omp parallel for num_threads( threads ) schedule( static )
2062  for( j=0 ; j<asf.adjacencyCount ; j++ )
2063  {
2064  B_[j] += sNodes.treeNodes[asf.adjacencies[j]]->nodeData.constraint;
2065  sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.constraint = 0;
2066  }
2067 
2068  // Solve the matrix
2069  // Since we don't have the full matrix, the system shouldn't be singular, so we shouldn't have to correct it
2070  iter += SparseSymmetricMatrix< Real >::Solve( _M , B_ , std::max< int >( int( pow( _M.rows , ITERATION_POWER ) ) , minIters ) , X_ , mrVector , Real(accuracy) , 0 );
2071 
2072  if( showResidual )
2073  {
2074  double mNorm = 0;
2075  for( int i=0 ; i<_M.rows ; i++ ) for( int j=0 ; j<_M.rowSizes[i] ; j++ ) mNorm += _M[i][j].Value * _M[i][j].Value;
2076  double bNorm = B_.Norm( 2 ) , rNorm = ( B_ - _M * X_ ).Norm( 2 );
2077  printf( "\t\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , _M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter );
2078  }
2079 
2080  // Update the solution for all nodes in the sub-tree
2081  for( j=0 ; j<asf.adjacencyCount ; j++ )
2082  {
2083  TreeOctNode* temp=sNodes.treeNodes[ asf.adjacencies[j] ];
2084  while( temp->depth()>sNodes.treeNodes[i]->depth() ) temp=temp->parent;
2085  if( temp->nodeData.nodeIndex>=sNodes.treeNodes[i]->nodeData.nodeIndex ) sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution = Real( X_[j] );
2086  }
2087  systemTime += gTime;
2088  solveTime += sTime;
2089  memUsage = std::max< double >( MemoryUsage() , memUsage );
2090  tIter += iter;
2091  }
2092  delete[] asf.adjacencies;
2093  return tIter;
2094  }
2095  template<int Degree>
2096  int Octree<Degree>::HasNormals(TreeOctNode* node,Real epsilon)
2097  {
2098  int hasNormals=0;
2099  if( node->nodeData.normalIndex>=0 && ( (*normals)[node->nodeData.normalIndex][0]!=0 || (*normals)[node->nodeData.normalIndex][1]!=0 || (*normals)[node->nodeData.normalIndex][2]!=0 ) ) hasNormals=1;
2100  if( node->children ) for(int i=0;i<Cube::CORNERS && !hasNormals;i++) hasNormals |= HasNormals(&node->children[i],epsilon);
2101 
2102  return hasNormals;
2103  }
2104  template<int Degree>
2106  {
2107  int maxDepth = tree.maxDepth();
2108  for( TreeOctNode* temp=tree.nextNode() ; temp ; temp=tree.nextNode(temp) )
2109  if( temp->children && temp->d>=_minDepth )
2110  {
2111  int hasNormals=0;
2112  for( int i=0 ; i<Cube::CORNERS && !hasNormals ; i++ ) hasNormals = HasNormals( &temp->children[i] , EPSILON/(1<<maxDepth) );
2113  if( !hasNormals ) temp->children=NULL;
2114  }
2115  MemoryUsage();
2116  }
2117  template<int Degree>
2119  {
2120  // To set the Laplacian constraints, we iterate over the
2121  // splatted normals and compute the dot-product of the
2122  // divergence of the normal field with all the basis functions.
2123  // Within the same depth: set directly as a gather
2124  // Coarser depths
2125  fData.setDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG );
2126 
2127  int maxDepth = _sNodes.maxDepth-1;
2128  Point3D< Real > zeroPoint;
2129  zeroPoint[0] = zeroPoint[1] = zeroPoint[2] = 0;
2130  std::vector< Real > constraints( _sNodes.nodeCount[maxDepth] , Real(0) );
2131  std::vector< Point3D< Real > > coefficients( _sNodes.nodeCount[maxDepth] , zeroPoint );
2132 
2133  // Clear the constraints
2134 #pragma omp parallel for num_threads( threads )
2135  for( int i=0 ; i<_sNodes.nodeCount[maxDepth+1] ; i++ ) _sNodes.treeNodes[i]->nodeData.constraint = Real( 0. );
2136 
2137  // For the scattering part of the operation, we parallelize by duplicating the constraints and then summing at the end.
2138  std::vector< std::vector< Real > > _constraints( threads );
2139  for( int t=0 ; t<threads ; t++ ) _constraints[t].resize( _sNodes.nodeCount[maxDepth] , 0 );
2140 
2141  for( int d=maxDepth ; d>=0 ; d-- )
2142  {
2143  Point3D< double > stencil[5][5][5];
2144  SetDivergenceStencil( d , &stencil[0][0][0] , false );
2145  Stencil< Point3D< double > , 5 > stencils[2][2][2];
2146  SetDivergenceStencils( d , stencils , true );
2147 #pragma omp parallel for num_threads( threads )
2148  for( int t=0 ; t<threads ; t++ )
2149  {
2150  TreeOctNode::NeighborKey5 neighborKey5;
2151  neighborKey5.set( fData.depth );
2152  int start = _sNodes.nodeCount[d] , end = _sNodes.nodeCount[d+1] , range = end-start;
2153  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
2154  {
2155  TreeOctNode* node = _sNodes.treeNodes[i];
2156  int startX=0 , endX=5 , startY=0 , endY=5 , startZ=0 , endZ=5;
2157  int depth = node->depth();
2158  neighborKey5.getNeighbors( node );
2159 
2160  bool isInterior , isInterior2;
2161  {
2162  int d , off[3];
2163  node->depthAndOffset( d , off );
2164  int mn = 2 , mx = (1<<d)-2;
2165  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
2166  mn += 2 , mx -= 2;
2167  isInterior2 = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
2168  }
2169  int cx , cy , cz;
2170  if( d )
2171  {
2172  int c = int( node - node->parent->children );
2173  Cube::FactorCornerIndex( c , cx , cy , cz );
2174  }
2175  else cx = cy = cz = 0;
2176  Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz];
2177 
2178  // Set constraints from current depth
2179  {
2180  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth];
2181 
2182  if( isInterior )
2183  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2184  {
2185  const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
2186  if( _node && _node->nodeData.normalIndex>=0 )
2187  {
2188  const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex];
2189  node->nodeData.constraint += Real( stencil[x][y][z][0] * _normal[0] + stencil[x][y][z][1] * _normal[1] + stencil[x][y][z][2] * _normal[2] );
2190  }
2191  }
2192  else
2193  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2194  {
2195  const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
2196  if( _node && _node->nodeData.normalIndex>=0 )
2197  {
2198  const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex];
2199  node->nodeData.constraint += GetDivergence( _node , node , _normal );
2200  }
2201  }
2202  UpdateCoarserSupportBounds( neighbors5.neighbors[2][2][2] , startX , endX , startY , endY , startZ , endZ );
2203  }
2204  if( node->nodeData.nodeIndex<0 || node->nodeData.normalIndex<0 ) continue;
2205  const Point3D< Real >& normal = (*normals)[node->nodeData.normalIndex];
2206  if( normal[0]==0 && normal[1]==0 && normal[2]==0 ) continue;
2207  if( depth<maxDepth ) coefficients[i] += normal;
2208 
2209  if( depth )
2210  {
2211  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth-1];
2212 
2213  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2214  if( neighbors5.neighbors[x][y][z] )
2215  {
2216  TreeOctNode* _node = neighbors5.neighbors[x][y][z];
2217  if( isInterior2 )
2218  {
2219  Point3D< double >& div = _stencil.values[x][y][z];
2220  _constraints[t][ _node->nodeData.nodeIndex ] += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] );
2221  }
2222  else _constraints[t][ _node->nodeData.nodeIndex ] += GetDivergence( node , _node , normal );
2223  }
2224  }
2225  }
2226  }
2227  }
2228 #pragma omp parallel for num_threads( threads ) schedule( static )
2229  for( int i=0 ; i<_sNodes.nodeCount[maxDepth] ; i++ )
2230  {
2231  Real cSum = Real(0.);
2232  for( int t=0 ; t<threads ; t++ ) cSum += _constraints[t][i];
2233  constraints[i] = cSum;
2234  }
2235  // Fine-to-coarse down-sampling of constraints
2236  for( int d=maxDepth-1 ; d>=0 ; d-- ) DownSample( d , _sNodes , &constraints[0] );
2237 
2238  // Coarse-to-fine up-sampling of coefficients
2239  for( int d=0 ; d<maxDepth ; d++ ) UpSample( d , _sNodes , &coefficients[0] );
2240 
2241  // Add the accumulated constraints from all finer depths
2242 #pragma omp parallel for num_threads( threads )
2243  for( int i=0 ; i<_sNodes.nodeCount[maxDepth] ; i++ ) _sNodes.treeNodes[i]->nodeData.constraint += constraints[i];
2244 
2245  // Compute the contribution from all coarser depths
2246  for( int d=0 ; d<=maxDepth ; d++ )
2247  {
2248  int start = _sNodes.nodeCount[d] , end = _sNodes.nodeCount[d+1] , range = end - start;
2249  Stencil< Point3D< double > , 5 > stencils[2][2][2];
2250  SetDivergenceStencils( d , stencils , false );
2251 #pragma omp parallel for num_threads( threads )
2252  for( int t=0 ; t<threads ; t++ )
2253  {
2254  TreeOctNode::NeighborKey5 neighborKey5;
2255  neighborKey5.set( maxDepth );
2256  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
2257  {
2258  TreeOctNode* node = _sNodes.treeNodes[i];
2259  int depth = node->depth();
2260  if( !depth ) continue;
2261  int startX=0 , endX=5 , startY=0 , endY=5 , startZ=0 , endZ=5;
2262  UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ );
2263  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.getNeighbors( node->parent );
2264 
2265  bool isInterior;
2266  {
2267  int d , off[3];
2268  node->depthAndOffset( d , off );
2269  int mn = 4 , mx = (1<<d)-4;
2270  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
2271  }
2272  int cx , cy , cz;
2273  if( d )
2274  {
2275  int c = int( node - node->parent->children );
2276  Cube::FactorCornerIndex( c , cx , cy , cz );
2277  }
2278  else cx = cy = cz = 0;
2279  Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz];
2280 
2281  Real constraint = Real(0);
2282  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2283  if( neighbors5.neighbors[x][y][z] )
2284  {
2285  TreeOctNode* _node = neighbors5.neighbors[x][y][z];
2286  int _i = _node->nodeData.nodeIndex;
2287  if( isInterior )
2288  {
2289  Point3D< double >& div = _stencil.values[x][y][z];
2290  Point3D< Real >& normal = coefficients[_i];
2291  constraint += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] );
2292  }
2293  else constraint += GetDivergence( _node , node , coefficients[_i] );
2294  }
2295  node->nodeData.constraint += constraint;
2296  }
2297  }
2298  }
2299 
2300  fData.clearDotTables( fData.DV_DOT_FLAG );
2301 
2302  // Set the point weights for evaluating the iso-value
2303 #pragma omp parallel for num_threads( threads )
2304  for( int t=0 ; t<threads ; t++ )
2305  for( int i=(_sNodes.nodeCount[maxDepth+1]*t)/threads ; i<(_sNodes.nodeCount[maxDepth+1]*(t+1))/threads ; i++ )
2306  {
2307  TreeOctNode* temp = _sNodes.treeNodes[i];
2308  if( temp->nodeData.nodeIndex<0 || temp->nodeData.normalIndex<0 ) temp->nodeData.centerWeightContribution = 0;
2309  else temp->nodeData.centerWeightContribution = Real( Length((*normals)[temp->nodeData.normalIndex]) );
2310  }
2311  MemoryUsage();
2312  delete normals;
2313  normals = NULL;
2314  }
2315  template<int Degree>
2316  void Octree<Degree>::AdjacencyCountFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencyCount++;}
2317  template<int Degree>
2318  void Octree<Degree>::AdjacencySetFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencies[adjacencyCount++]=node1->nodeData.nodeIndex;}
2319  template<int Degree>
2321  {
2322  if( !node1->children && node1->depth()<depth ) node1->initChildren();
2323  }
2324  template< int Degree >
2325  void Octree< Degree >::FaceEdgesFunction::Function( const TreeOctNode* node1 , const TreeOctNode* node2 )
2326  {
2327  if( !node1->children && MarchingCubes::HasRoots( node1->nodeData.mcIndex ) )
2328  {
2329  RootInfo ri1 , ri2;
2330  int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES];
2331  int count=MarchingCubes::AddTriangleIndices( node1->nodeData.mcIndex , isoTri );
2332 
2333  for( int j=0 ; j<count ; j++ )
2334  for( int k=0 ; k<3 ; k++ )
2335  if( fIndex==Cube::FaceAdjacentToEdges( isoTri[j*3+k] , isoTri[j*3+((k+1)%3)] ) )
2336  if( GetRootIndex( node1 , isoTri[j*3+k] , maxDepth , ri1 ) && GetRootIndex( node1 , isoTri[j*3+((k+1)%3)] , maxDepth , ri2 ) )
2337  {
2338  long long key1=ri1.key , key2=ri2.key;
2339  edges->push_back( std::pair< RootInfo , RootInfo >( ri2 , ri1 ) );
2340  if( vertexCount->count( key1 )==0 )
2341  {
2342  (*vertexCount)[key1].first = ri1;
2343  (*vertexCount)[key1].second=0;
2344  }
2345  if( vertexCount->count( key2 )==0 )
2346  {
2347  (*vertexCount)[key2].first = ri2;
2348  (*vertexCount)[key2].second=0;
2349  }
2350  (*vertexCount)[key1].second--;
2351  (*vertexCount)[key2].second++;
2352  }
2353  else fprintf( stderr , "Bad Edge 1: %d %d\n" , ri1.key , ri2.key );
2354  }
2355  }
2356 
2357  template< int Degree >
2358  void Octree< Degree >::RefineBoundary( int subdivideDepth )
2359  {
2360  // This implementation is somewhat tricky.
2361  // We would like to ensure that leaf-nodes across a subdivision boundary have the same depth.
2362  // We do this by calling the setNeighbors function.
2363  // The key is to implement this in a single pass through the leaves, ensuring that refinements don't propogate.
2364  // To this end, we do the minimal refinement that ensures that a cross boundary neighbor, and any of its cross-boundary
2365  // neighbors are all refined simultaneously.
2366  // For this reason, the implementation can only support nodes deeper than sDepth.
2367  bool flags[3][3][3];
2368  int maxDepth = tree.maxDepth();
2369 
2370  int sDepth;
2371  if( subdivideDepth<=0 ) sDepth = 0;
2372  else sDepth = maxDepth-subdivideDepth;
2373  if( sDepth<=0 ) return;
2374 
2375  // Ensure that face adjacent neighbors across the subdivision boundary exist to allow for
2376  // a consistent definition of the iso-surface
2377  TreeOctNode::NeighborKey3 nKey;
2378  nKey.set( maxDepth );
2379  for( TreeOctNode* leaf=tree.nextLeaf() ; leaf ; leaf=tree.nextLeaf( leaf ) )
2380  if( leaf->depth()>sDepth )
2381  {
2382  int d , off[3] , _off[3];
2383  leaf->depthAndOffset( d , off );
2384  int res = (1<<d)-1 , _res = ( 1<<(d-sDepth) )-1;
2385  _off[0] = off[0]&_res , _off[1] = off[1]&_res , _off[2] = off[2]&_res;
2386  bool boundary[3][2] =
2387  {
2388  { (off[0]!=0 && _off[0]==0) , (off[0]!=res && _off[0]==_res) } ,
2389  { (off[1]!=0 && _off[1]==0) , (off[1]!=res && _off[1]==_res) } ,
2390  { (off[2]!=0 && _off[2]==0) , (off[2]!=res && _off[2]==_res) }
2391  };
2392 
2393  if( boundary[0][0] || boundary[0][1] || boundary[1][0] || boundary[1][1] || boundary[2][0] || boundary[2][1] )
2394  {
2395  TreeOctNode::Neighbors3& neighbors = nKey.getNeighbors( leaf );
2396  for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) flags[i][j][k] = false;
2397  int x=0 , y=0 , z=0;
2398  if ( boundary[0][0] && !neighbors.neighbors[0][1][1] ) x = -1;
2399  else if( boundary[0][1] && !neighbors.neighbors[2][1][1] ) x = 1;
2400  if ( boundary[1][0] && !neighbors.neighbors[1][0][1] ) y = -1;
2401  else if( boundary[1][1] && !neighbors.neighbors[1][2][1] ) y = 1;
2402  if ( boundary[2][0] && !neighbors.neighbors[1][1][0] ) z = -1;
2403  else if( boundary[2][1] && !neighbors.neighbors[1][1][2] ) z = 1;
2404 
2405  if( x || y || z )
2406  {
2407  // Corner case
2408  if( x && y && z ) flags[1+x][1+y][1+z] = true;
2409  // Edge cases
2410  if( x && y ) flags[1+x][1+y][1 ] = true;
2411  if( x && z ) flags[1+x][1 ][1+z] = true;
2412  if( y && z ) flags[1 ][1+y][1+1] = true;
2413  // Face cases
2414  if( x ) flags[1+x][1 ][1 ] = true;
2415  if( y ) flags[1 ][1+y][1 ] = true;
2416  if( z ) flags[1 ][1 ][1+z] = true;
2417  nKey.setNeighbors( leaf , flags );
2418  }
2419  }
2420  }
2421  _sNodes.set( tree );
2422  MemoryUsage();
2423  }
2424  template<int Degree>
2425  void Octree<Degree>::GetMCIsoTriangles( Real isoValue , int subdivideDepth , CoredMeshData* mesh , int fullDepthIso , int nonLinearFit , bool addBarycenter , bool polygonMesh )
2426  {
2427  fData.setValueTables( fData.VALUE_FLAG | fData.D_VALUE_FLAG , 0 , postNormalSmooth );
2428 
2429  // Ensure that the subtrees are self-contained
2430  RefineBoundary( subdivideDepth );
2431 
2432  RootData rootData , coarseRootData;
2433  std::vector< Point3D< float > >* interiorPoints;
2434  int maxDepth = tree.maxDepth();
2435 
2436  int sDepth = subdivideDepth<=0 ? 0 : std::max< int >( 0 , maxDepth-subdivideDepth );
2437 
2438  std::vector< Real > metSolution( _sNodes.nodeCount[maxDepth] , 0 );
2439 #pragma omp parallel for num_threads( threads )
2440  for( int i=_sNodes.nodeCount[_minDepth] ; i<_sNodes.nodeCount[maxDepth] ; i++ ) metSolution[i] = _sNodes.treeNodes[i]->nodeData.solution;
2441  for( int d=0 ; d<maxDepth ; d++ ) UpSample( d , _sNodes , &metSolution[0] );
2442 
2443  // Clear the marching cube indices
2444 #pragma omp parallel for num_threads( threads )
2445  for( int i=0 ; i<_sNodes.nodeCount[maxDepth+1] ; i++ ) _sNodes.treeNodes[i]->nodeData.mcIndex = 0;
2446 
2447  rootData.boundaryValues = new std::unordered_map< long long , std::pair< Real , Point3D< Real > > >();
2448  int offSet = 0;
2449 
2450  int maxCCount = _sNodes.getMaxCornerCount( &tree , sDepth , maxDepth , threads );
2451  int maxECount = _sNodes.getMaxEdgeCount ( &tree , sDepth , threads );
2452  rootData.cornerValues = new Real [ maxCCount ];
2453  rootData.cornerNormals = new Point3D< Real >[ maxCCount ];
2454  rootData.interiorRoots = new int [ maxECount ];
2455  rootData.cornerValuesSet = new char[ maxCCount ];
2456  rootData.cornerNormalsSet = new char[ maxCCount ];
2457  rootData.edgesSet = new char[ maxECount ];
2458  _sNodes.setCornerTable( coarseRootData , &tree , sDepth , threads );
2459  coarseRootData.cornerValues = new Real[ coarseRootData.cCount ];
2460  coarseRootData.cornerNormals = new Point3D< Real >[ coarseRootData.cCount ];
2461  coarseRootData.cornerValuesSet = new char[ coarseRootData.cCount ];
2462  coarseRootData.cornerNormalsSet = new char[ coarseRootData.cCount ];
2463  memset( coarseRootData.cornerValuesSet , 0 , sizeof( char ) * coarseRootData.cCount );
2464  memset( coarseRootData.cornerNormalsSet , 0 , sizeof( char ) * coarseRootData.cCount );
2465  MemoryUsage();
2466 
2467  std::vector< TreeOctNode::ConstNeighborKey3 > nKeys( threads );
2468  for( int t=0 ; t<threads ; t++ ) nKeys[t].set( maxDepth );
2469  TreeOctNode::ConstNeighborKey3 nKey;
2470  std::vector< TreeOctNode::ConstNeighborKey5 > nKeys5( threads );
2471  for( int t=0 ; t<threads ; t++ ) nKeys5[t].set( maxDepth );
2472  TreeOctNode::ConstNeighborKey5 nKey5;
2473  nKey5.set( maxDepth );
2474  nKey.set( maxDepth );
2475  // First process all leaf nodes at depths strictly finer than sDepth, one subtree at a time.
2476  for( int i=_sNodes.nodeCount[sDepth] ; i<_sNodes.nodeCount[sDepth+1] ; i++ )
2477  {
2478  if( !_sNodes.treeNodes[i]->children ) continue;
2479 
2480  _sNodes.setCornerTable( rootData , _sNodes.treeNodes[i] , threads );
2481  _sNodes.setEdgeTable ( rootData , _sNodes.treeNodes[i] , threads );
2482  memset( rootData.cornerValuesSet , 0 , sizeof( char ) * rootData.cCount );
2483  memset( rootData.cornerNormalsSet , 0 , sizeof( char ) * rootData.cCount );
2484  memset( rootData.edgesSet , 0 , sizeof( char ) * rootData.eCount );
2485  interiorPoints = new std::vector< Point3D< float > >();
2486  for( int d=maxDepth ; d>sDepth ; d-- )
2487  {
2488  int leafNodeCount = 0;
2489  std::vector< TreeOctNode* > leafNodes;
2490  for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodeCount++;
2491  leafNodes.reserve( leafNodeCount );
2492  for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodes.push_back( node );
2493  Stencil< double , 3 > stencil1[8] , stencil2[8][8];
2494  SetEvaluationStencils( d , stencil1 , stencil2 );
2495 
2496  // First set the corner values and associated marching-cube indices
2497 #pragma omp parallel for num_threads( threads )
2498  for( int t=0 ; t<threads ; t++ ) for( int i=(leafNodeCount*t)/threads ; i<(leafNodeCount*(t+1))/threads ; i++ )
2499  {
2500  TreeOctNode* leaf = leafNodes[i];
2501  SetIsoCorners( isoValue , leaf , rootData , rootData.cornerValuesSet , rootData.cornerValues , nKeys[t] , &metSolution[0] , stencil1 , stencil2 );
2502 
2503  // If this node shares a vertex with a coarser node, set the vertex value
2504  int d , off[3];
2505  leaf->depthAndOffset( d , off );
2506  int res = 1<<(d-sDepth);
2507  off[0] %= res , off[1] %=res , off[2] %= res;
2508  res--;
2509  if( !(off[0]%res) && !(off[1]%res) && !(off[2]%res) )
2510  {
2511  const TreeOctNode* temp = leaf;
2512  while( temp->d!=sDepth ) temp = temp->parent;
2513  int x = off[0]==0 ? 0 : 1 , y = off[1]==0 ? 0 : 1 , z = off[2]==0 ? 0 : 1;
2514  int c = Cube::CornerIndex( x , y , z );
2515  int idx = coarseRootData.cornerIndices( temp )[ c ];
2516  coarseRootData.cornerValues[ idx ] = rootData.cornerValues[ rootData.cornerIndices( leaf )[c] ];
2517  coarseRootData.cornerValuesSet[ idx ] = true;
2518  }
2519 
2520  // Compute the iso-vertices
2522  SetMCRootPositions( leaf , sDepth , isoValue , nKeys5[t] , rootData , interiorPoints , mesh , &metSolution[0] , nonLinearFit );
2523  }
2524  // Note that this should be broken off for multi-threading as
2525  // the SetMCRootPositions writes to interiorPoints (with lockupdateing)
2526  // while GetMCIsoTriangles reads from interiorPoints (without locking)
2527 #if MISHA_DEBUG
2528  std::vector< Point3D< float > > barycenters;
2529  std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? & barycenters : NULL;
2530 #endif // MISHA_DEBUG
2531 #pragma omp parallel for num_threads( threads )
2532  for( int t=0 ; t<threads ; t++ ) for( int i=(leafNodeCount*t)/threads ; i<(leafNodeCount*(t+1))/threads ; i++ )
2533  {
2534  TreeOctNode* leaf = leafNodes[i];
2536 #if MISHA_DEBUG
2537  GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , polygonMesh , barycenterPtr );
2538 #else // !MISHA_DEBUG
2539  GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , addBarycenter , polygonMesh );
2540 #endif // MISHA_DEBUG
2541  }
2542 #if MISHA_DEBUG
2543  for( int i=0 ; i<barycenters.size() ; i++ ) interiorPoints->push_back( barycenters[i] );
2544 #endif // MISHA_DEBUG
2545  }
2546  offSet = mesh->outOfCorePointCount();
2547 #if 1
2548  delete interiorPoints;
2549 #endif
2550  }
2551  MemoryUsage();
2552  delete[] rootData.cornerValues , delete[] rootData.cornerNormals , rootData.cornerValues = NULL , rootData.cornerNormals = NULL;
2553  delete[] rootData.cornerValuesSet , delete[] rootData.cornerNormalsSet , rootData.cornerValuesSet = NULL , rootData.cornerNormalsSet = NULL;
2554  delete[] rootData.interiorRoots ; rootData.interiorRoots = NULL;
2555  delete[] rootData.edgesSet ; rootData.edgesSet = NULL;
2556  coarseRootData.interiorRoots = NULL;
2557  coarseRootData.boundaryValues = rootData.boundaryValues;
2558  for( auto iter=rootData.boundaryRoots.cbegin() ; iter!=rootData.boundaryRoots.cend() ; iter++ )
2559  coarseRootData.boundaryRoots[iter->first] = iter->second;
2560 
2561  for( int d=sDepth ; d>=0 ; d-- )
2562  {
2563  Stencil< double , 3 > stencil1[8] , stencil2[8][8];
2564  SetEvaluationStencils( d , stencil1 , stencil2 );
2565 #if MISHA_DEBUG
2566  std::vector< Point3D< float > > barycenters;
2567  std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? &barycenters : NULL;
2568 #endif // MISHA_DEBUG
2569  for( int i=_sNodes.nodeCount[d] ; i<_sNodes.nodeCount[d+1] ; i++ )
2570  {
2571  TreeOctNode* leaf = _sNodes.treeNodes[i];
2572  if( leaf->children ) continue;
2573 
2574  // First set the corner values and associated marching-cube indices
2575  SetIsoCorners( isoValue , leaf , coarseRootData , coarseRootData.cornerValuesSet , coarseRootData.cornerValues , nKey , &metSolution[0] , stencil1 , stencil2 );
2576 
2577  // Now compute the iso-vertices
2579  {
2580  SetMCRootPositions( leaf , 0 , isoValue , nKey5 , coarseRootData , NULL , mesh , &metSolution[0] , nonLinearFit );
2581 #if MISHA_DEBUG
2582  GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , polygonMesh , barycenterPtr );
2583 #else // !MISHA_DEBUG
2584  GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , addBarycenter , polygonMesh );
2585 #endif // MISHA_DEBUG
2586  }
2587  }
2588  }
2589  MemoryUsage();
2590 
2591  delete[] coarseRootData.cornerValues , delete[] coarseRootData.cornerNormals;
2592  delete[] coarseRootData.cornerValuesSet , delete[] coarseRootData.cornerNormalsSet;
2593  delete rootData.boundaryValues;
2594  }
2595  template<int Degree>
2597  int idx[3];
2598  Real value=0;
2599 
2600  VertexData::CenterIndex(node,fData.depth,idx);
2601  idx[0]*=fData.functionCount;
2602  idx[1]*=fData.functionCount;
2603  idx[2]*=fData.functionCount;
2604  int minDepth = std::max< int >( 0 , std::min< int >( _minDepth , node->depth()-1 ) );
2605  for( int i=minDepth ; i<=node->depth() ; i++ )
2606  for(int j=0;j<3;j++)
2607  for(int k=0;k<3;k++)
2608  for(int l=0;l<3;l++)
2609  {
2610  const TreeOctNode* n=neighborKey.neighbors[i].neighbors[j][k][l];
2611  if( n )
2612  {
2613  Real temp=n->nodeData.solution;
2614  value+=temp*Real(
2615  fData.valueTables[idx[0]+int(n->off[0])]*
2616  fData.valueTables[idx[1]+int(n->off[1])]*
2617  fData.valueTables[idx[2]+int(n->off[2])]);
2618  }
2619  }
2620  if(node->children)
2621  {
2622  for(int i=0;i<Cube::CORNERS;i++){
2623  int ii=Cube::AntipodalCornerIndex(i);
2624  const TreeOctNode* n=&node->children[i];
2625  while(1){
2626  value+=n->nodeData.solution*Real(
2627  fData.valueTables[idx[0]+int(n->off[0])]*
2628  fData.valueTables[idx[1]+int(n->off[1])]*
2629  fData.valueTables[idx[2]+int(n->off[2])]);
2630  if( n->children ) n=&n->children[ii];
2631  else break;
2632  }
2633  }
2634  }
2635  return value;
2636  }
2637  template< int Degree >
2638  Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution )
2639  {
2640  int idx[3];
2641  double value = 0;
2642 
2643  VertexData::CornerIndex( node , corner , fData.depth , idx );
2644  idx[0] *= fData.functionCount;
2645  idx[1] *= fData.functionCount;
2646  idx[2] *= fData.functionCount;
2647 
2648  int d = node->depth();
2649  int cx , cy , cz;
2650  int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3;
2651  Cube::FactorCornerIndex( corner , cx , cy , cz );
2652  {
2653  TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d];
2654  if( cx==0 ) endX = 2;
2655  else startX = 1;
2656  if( cy==0 ) endY = 2;
2657  else startY = 1;
2658  if( cz==0 ) endZ = 2;
2659  else startZ = 1;
2660  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2661  {
2662  const TreeOctNode* n=neighbors.neighbors[x][y][z];
2663  if( n )
2664  {
2665  double v =
2666  fData.valueTables[ idx[0]+int(n->off[0]) ]*
2667  fData.valueTables[ idx[1]+int(n->off[1]) ]*
2668  fData.valueTables[ idx[2]+int(n->off[2]) ];
2669  value += n->nodeData.solution * v;
2670  }
2671  }
2672  }
2673  if( d>0 && d>_minDepth )
2674  {
2675  int _corner = int( node - node->parent->children );
2676  int _cx , _cy , _cz;
2677  Cube::FactorCornerIndex( _corner , _cx , _cy , _cz );
2678  if( cx!=_cx ) startX = 0 , endX = 3;
2679  if( cy!=_cy ) startY = 0 , endY = 3;
2680  if( cz!=_cz ) startZ = 0 , endZ = 3;
2681  TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1];
2682  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2683  {
2684  const TreeOctNode* n=neighbors.neighbors[x][y][z];
2685  if( n )
2686  {
2687  double v =
2688  fData.valueTables[ idx[0]+int(n->off[0]) ]*
2689  fData.valueTables[ idx[1]+int(n->off[1]) ]*
2690  fData.valueTables[ idx[2]+int(n->off[2]) ];
2691  value += metSolution[ n->nodeData.nodeIndex ] * v;
2692  }
2693  }
2694  }
2695  return Real( value );
2696  }
2697  template< int Degree >
2698  Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution , const double stencil1[3][3][3] , const double stencil2[3][3][3] )
2699  {
2700  double value = 0;
2701  int d = node->depth();
2702  int cx , cy , cz;
2703  int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3;
2704  Cube::FactorCornerIndex( corner , cx , cy , cz );
2705  {
2706  TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d];
2707  if( cx==0 ) endX = 2;
2708  else startX = 1;
2709  if( cy==0 ) endY = 2;
2710  else startY = 1;
2711  if( cz==0 ) endZ = 2;
2712  else startZ = 1;
2713  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2714  {
2715  const TreeOctNode* n=neighbors.neighbors[x][y][z];
2716  if( n ) value += n->nodeData.solution * stencil1[x][y][z];
2717  }
2718  }
2719  if( d>0 && d>_minDepth )
2720  {
2721  int _corner = int( node - node->parent->children );
2722  int _cx , _cy , _cz;
2723  Cube::FactorCornerIndex( _corner , _cx , _cy , _cz );
2724  if( cx!=_cx ) startX = 0 , endX = 3;
2725  if( cy!=_cy ) startY = 0 , endY = 3;
2726  if( cz!=_cz ) startZ = 0 , endZ = 3;
2727  TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1];
2728  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2729  {
2730  const TreeOctNode* n=neighbors.neighbors[x][y][z];
2731  if( n ) value += metSolution[ n->nodeData.nodeIndex ] * stencil2[x][y][z];
2732  }
2733  }
2734  return Real( value );
2735  }
2736  template< int Degree >
2737  Point3D< Real > Octree< Degree >::getCornerNormal( const OctNode< TreeNodeData , Real >::ConstNeighborKey5& neighborKey5 , const TreeOctNode* node , int corner , const Real* metSolution )
2738  {
2739  int idx[3];
2740  Point3D< Real > normal;
2741  normal[0] = normal[1] = normal[2] = 0.;
2742 
2743  VertexData::CornerIndex( node , corner , fData.depth , idx );
2744  idx[0] *= fData.functionCount;
2745  idx[1] *= fData.functionCount;
2746  idx[2] *= fData.functionCount;
2747 
2748  int d = node->depth();
2749  // Iterate over all ancestors that can overlap the corner
2750  {
2751  TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d];
2752  for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ )
2753  {
2754  const TreeOctNode* n=neighbors.neighbors[j][k][l];
2755  if( n )
2756  {
2757  int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] };
2758  double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] };
2759  double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] };
2760  Real solution = n->nodeData.solution;
2761  normal[0] += Real( dValues[0] * values[1] * values[2] * solution );
2762  normal[1] += Real( values[0] * dValues[1] * values[2] * solution );
2763  normal[2] += Real( values[0] * values[1] * dValues[2] * solution );
2764  }
2765  }
2766  }
2767  if( d>0 && d>_minDepth )
2768  {
2769  TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d-1];
2770  for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ )
2771  {
2772  const TreeOctNode* n=neighbors.neighbors[j][k][l];
2773  if( n )
2774  {
2775  int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] };
2776  double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] };
2777  double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] };
2778  Real solution = metSolution[ n->nodeData.nodeIndex ];
2779  normal[0] += Real( dValues[0] * values[1] * values[2] * solution );
2780  normal[1] += Real( values[0] * dValues[1] * values[2] * solution );
2781  normal[2] += Real( values[0] * values[1] * dValues[2] * solution );
2782  }
2783  }
2784  }
2785  return normal;
2786  }
2787  template< int Degree >
2789  {
2790  Real isoValue , weightSum;
2791 
2792  neighborKey2.set( fData.depth );
2793  fData.setValueTables( fData.VALUE_FLAG , 0 );
2794 
2795  isoValue = weightSum = 0;
2796 #pragma omp parallel for num_threads( threads ) reduction( + : isoValue , weightSum )
2797  for( int t=0 ; t<threads ; t++)
2798  {
2799  TreeOctNode::ConstNeighborKey3 nKey;
2800  nKey.set( _sNodes.maxDepth-1 );
2801  int nodeCount = _sNodes.nodeCount[ _sNodes.maxDepth ];
2802  for( int i=(nodeCount*t)/threads ; i<(nodeCount*(t+1))/threads ; i++ )
2803  {
2804  TreeOctNode* temp = _sNodes.treeNodes[i];
2805  nKey.getNeighbors( temp );
2807  if( w!=0 )
2808  {
2809  isoValue += getCenterValue( nKey , temp ) * w;
2810  weightSum += w;
2811  }
2812  }
2813  }
2814  return isoValue/weightSum;
2815  }
2816 
2817  template< int Degree >
2818  void Octree< Degree >::SetIsoCorners( Real isoValue , TreeOctNode* leaf , SortedTreeNodes::CornerTableData& cData , char* valuesSet , Real* values , TreeOctNode::ConstNeighborKey3& nKey , const Real* metSolution , const Stencil< double , 3 > stencil1[8] , const Stencil< double , 3 > stencil2[8][8] )
2819  {
2820  Real cornerValues[ Cube::CORNERS ];
2821  const SortedTreeNodes::CornerIndices& cIndices = cData[ leaf ];
2822 
2823  bool isInterior;
2824  int d , off[3];
2825  leaf->depthAndOffset( d , off );
2826  int mn = 2 , mx = (1<<d)-2;
2827  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
2828  nKey.getNeighbors( leaf );
2829  for( int c=0 ; c<Cube::CORNERS ; c++ )
2830  {
2831  int vIndex = cIndices[c];
2832  if( valuesSet[vIndex] ) cornerValues[c] = values[vIndex];
2833  else
2834  {
2835  if( isInterior ) cornerValues[c] = getCornerValue( nKey , leaf , c , metSolution , stencil1[c].values , stencil2[int(leaf - leaf->parent->children)][c].values );
2836  else cornerValues[c] = getCornerValue( nKey , leaf , c , metSolution );
2837  values[vIndex] = cornerValues[c];
2838  valuesSet[vIndex] = 1;
2839  }
2840  }
2841 
2842  leaf->nodeData.mcIndex = MarchingCubes::GetIndex( cornerValues , isoValue );
2843 
2844  // Set the marching cube indices for all interior nodes.
2845  if( leaf->parent )
2846  {
2847  TreeOctNode* parent = leaf->parent;
2848  int c = int( leaf - leaf->parent->children );
2849  int mcid = leaf->nodeData.mcIndex & (1<<MarchingCubes::cornerMap()[c]);
2850 
2851  if( mcid )
2852  {
2853  poisson::atomicOr(parent->nodeData.mcIndex, mcid);
2854 
2855  while( 1 )
2856  {
2857  if( parent->parent && parent->parent->d>=_minDepth && (parent-parent->parent->children)==c )
2858  {
2859  poisson::atomicOr(parent->parent->nodeData.mcIndex, mcid);
2860  parent = parent->parent;
2861  }
2862  else break;
2863  }
2864  }
2865  }
2866  }
2867 
2868 
2869  template<int Degree>
2870  int Octree<Degree>::InteriorFaceRootCount(const TreeOctNode* node,const int &faceIndex,int maxDepth){
2871  int c1,c2,e1,e2,dir,off,cnt=0;
2872  int corners[Cube::CORNERS/2];
2873  if(node->children){
2874  Cube::FaceCorners(faceIndex,corners[0],corners[1],corners[2],corners[3]);
2875  Cube::FactorFaceIndex(faceIndex,dir,off);
2876  c1=corners[0];
2877  c2=corners[3];
2878  switch(dir){
2879  case 0:
2880  e1=Cube::EdgeIndex(1,off,1);
2881  e2=Cube::EdgeIndex(2,off,1);
2882  break;
2883  case 1:
2884  e1=Cube::EdgeIndex(0,off,1);
2885  e2=Cube::EdgeIndex(2,1,off);
2886  break;
2887  case 2:
2888  e1=Cube::EdgeIndex(0,1,off);
2889  e2=Cube::EdgeIndex(1,1,off);
2890  break;
2891  };
2892  cnt+=EdgeRootCount(&node->children[c1],e1,maxDepth)+EdgeRootCount(&node->children[c1],e2,maxDepth);
2893  switch(dir){
2894  case 0:
2895  e1=Cube::EdgeIndex(1,off,0);
2896  e2=Cube::EdgeIndex(2,off,0);
2897  break;
2898  case 1:
2899  e1=Cube::EdgeIndex(0,off,0);
2900  e2=Cube::EdgeIndex(2,0,off);
2901  break;
2902  case 2:
2903  e1=Cube::EdgeIndex(0,0,off);
2904  e2=Cube::EdgeIndex(1,0,off);
2905  break;
2906  };
2907  cnt+=EdgeRootCount(&node->children[c2],e1,maxDepth)+EdgeRootCount(&node->children[c2],e2,maxDepth);
2908  for(int i=0;i<Cube::CORNERS/2;i++){if(node->children[corners[i]].children){cnt+=InteriorFaceRootCount(&node->children[corners[i]],faceIndex,maxDepth);}}
2909  }
2910  return cnt;
2911  }
2912 
2913  template<int Degree>
2914  int Octree<Degree>::EdgeRootCount(const TreeOctNode* node,int edgeIndex,int maxDepth){
2915  int f1,f2,c1,c2;
2916  const TreeOctNode* temp;
2917  Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);
2918 
2919  int eIndex;
2920  const TreeOctNode* finest=node;
2921  eIndex=edgeIndex;
2922  if(node->depth()<maxDepth){
2923  temp=node->faceNeighbor(f1);
2924  if(temp && temp->children){
2925  finest=temp;
2926  eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1);
2927  }
2928  else{
2929  temp=node->faceNeighbor(f2);
2930  if(temp && temp->children){
2931  finest=temp;
2932  eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2);
2933  }
2934  else{
2935  temp=node->edgeNeighbor(edgeIndex);
2936  if(temp && temp->children){
2937  finest=temp;
2938  eIndex=Cube::EdgeReflectEdgeIndex(edgeIndex);
2939  }
2940  }
2941  }
2942  }
2943 
2944  Cube::EdgeCorners(eIndex,c1,c2);
2945  if(finest->children) return EdgeRootCount(&finest->children[c1],eIndex,maxDepth)+EdgeRootCount(&finest->children[c2],eIndex,maxDepth);
2946  else return MarchingCubes::HasEdgeRoots(finest->nodeData.mcIndex,eIndex);
2947  }
2948  template<int Degree>
2949  int Octree<Degree>::IsBoundaryFace(const TreeOctNode* node,int faceIndex,int subdivideDepth){
2950  int dir,offset,d,o[3],idx;
2951 
2952  if(subdivideDepth<0){return 0;}
2953  if(node->d<=subdivideDepth){return 1;}
2954  Cube::FactorFaceIndex(faceIndex,dir,offset);
2955  node->depthAndOffset(d,o);
2956 
2957  idx=(int(o[dir])<<1) + (offset<<1);
2958  return !(idx%(2<<(int(node->d)-subdivideDepth)));
2959  }
2960  template<int Degree>
2961  int Octree<Degree>::IsBoundaryEdge(const TreeOctNode* node,int edgeIndex,int subdivideDepth){
2962  int dir,x,y;
2963  Cube::FactorEdgeIndex(edgeIndex,dir,x,y);
2964  return IsBoundaryEdge(node,dir,x,y,subdivideDepth);
2965  }
2966  template<int Degree>
2967  int Octree<Degree>::IsBoundaryEdge( const TreeOctNode* node , int dir , int x , int y , int subdivideDepth )
2968  {
2969  int d , o[3] , idx1 , idx2 , mask;
2970 
2971  if( subdivideDepth<0 ) return 0;
2972  if( node->d<=subdivideDepth ) return 1;
2973  node->depthAndOffset( d , o );
2974 
2975  switch( dir )
2976  {
2977  case 0:
2978  idx1 = o[1] + x;
2979  idx2 = o[2] + y;
2980  break;
2981  case 1:
2982  idx1 = o[0] + x;
2983  idx2 = o[2] + y;
2984  break;
2985  case 2:
2986  idx1 = o[0] + x;
2987  idx2 = o[1] + y;
2988  break;
2989  }
2990  mask = 1<<( int(node->d) - subdivideDepth );
2991  return !(idx1%(mask)) || !(idx2%(mask));
2992  }
2993  template< int Degree >
2995  {
2996  int o , i1 , i2;
2997  Real width;
2998  Point3D< Real > c;
2999 
3000  Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 );
3001  ri.node->centerAndWidth( c , width );
3002  switch(o)
3003  {
3004  case 0:
3005  start[0] = c[0] - width/2;
3006  end [0] = c[0] + width/2;
3007  start[1] = end[1] = c[1] - width/2 + width*i1;
3008  start[2] = end[2] = c[2] - width/2 + width*i2;
3009  break;
3010  case 1:
3011  start[0] = end[0] = c[0] - width/2 + width*i1;
3012  start[1] = c[1] - width/2;
3013  end [1] = c[1] + width/2;
3014  start[2] = end[2] = c[2] - width/2 + width*i2;
3015  break;
3016  case 2:
3017  start[0] = end[0] = c[0] - width/2 + width*i1;
3018  start[1] = end[1] = c[1] - width/2 + width*i2;
3019  start[2] = c[2] - width/2;
3020  end [2] = c[2] + width/2;
3021  break;
3022  }
3023  }
3024  //////////////////////////////////////////////////////////////////////////////////////
3025  // The assumption made when calling this code is that the edge has at most one root //
3026  //////////////////////////////////////////////////////////////////////////////////////
3027  template< int Degree >
3028  int Octree< Degree >::GetRoot( const RootInfo& ri , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , Point3D< Real > & position , RootData& rootData , int sDepth , const Real* metSolution , int nonLinearFit )
3029  {
3030  if( !MarchingCubes::HasRoots( ri.node->nodeData.mcIndex ) ) return 0;
3031  int c1 , c2;
3032  Cube::EdgeCorners( ri.edgeIndex , c1 , c2 );
3033  if( !MarchingCubes::HasEdgeRoots( ri.node->nodeData.mcIndex , ri.edgeIndex ) ) return 0;
3034 
3035  long long key1 , key2;
3036  Point3D< Real > n[2];
3037 
3038  int i , o , i1 , i2 , rCount=0;
3039  Polynomial<2> P;
3040  std::vector< double > roots;
3041  double x0 , x1;
3042  Real center , width;
3043  Real averageRoot=0;
3044  Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 );
3045  int idx1[3] , idx2[3];
3046  key1 = VertexData::CornerIndex( ri.node , c1 , fData.depth , idx1 );
3047  key2 = VertexData::CornerIndex( ri.node , c2 , fData.depth , idx2 );
3048 
3049  bool isBoundary = ( IsBoundaryEdge( ri.node , ri.edgeIndex , sDepth )!=0 );
3050  bool haveKey1 , haveKey2;
3051  std::pair< Real , Point3D< Real > > keyValue1 , keyValue2;
3052  int iter1 , iter2;
3053  {
3054  iter1 = rootData.cornerIndices( ri.node )[ c1 ];
3055  iter2 = rootData.cornerIndices( ri.node )[ c2 ];
3056  keyValue1.first = rootData.cornerValues[iter1];
3057  keyValue2.first = rootData.cornerValues[iter2];
3058  if( isBoundary )
3059  {
3060 #pragma omp critical (normal_hash_access)
3061  {
3062  haveKey1 = ( rootData.boundaryValues->count( key1 )>0 );
3063  haveKey2 = ( rootData.boundaryValues->count( key2 )>0 );
3064  if( haveKey1 ) keyValue1 = (*rootData.boundaryValues)[key1];
3065  if( haveKey2 ) keyValue2 = (*rootData.boundaryValues)[key2];
3066  }
3067  }
3068  else
3069  {
3070  haveKey1 = ( rootData.cornerNormalsSet[ iter1 ] != 0 );
3071  haveKey2 = ( rootData.cornerNormalsSet[ iter2 ] != 0 );
3072  keyValue1.first = rootData.cornerValues[iter1];
3073  keyValue2.first = rootData.cornerValues[iter2];
3074  if( haveKey1 ) keyValue1.second = rootData.cornerNormals[iter1];
3075  if( haveKey2 ) keyValue2.second = rootData.cornerNormals[iter2];
3076  }
3077  }
3078  if( !haveKey1 || !haveKey2 ) neighborKey5.getNeighbors( ri.node );
3079  if( !haveKey1 ) keyValue1.second = getCornerNormal( neighborKey5 , ri.node , c1 , metSolution );
3080  x0 = keyValue1.first;
3081  n[0] = keyValue1.second;
3082 
3083  if( !haveKey2 ) keyValue2.second = getCornerNormal( neighborKey5 , ri.node , c2 , metSolution );
3084  x1 = keyValue2.first;
3085  n[1] = keyValue2.second;
3086 
3087  if( !haveKey1 || !haveKey2 )
3088  {
3089  if( isBoundary )
3090  {
3091 #pragma omp critical (normal_hash_access)
3092  {
3093  if( !haveKey1 ) (*rootData.boundaryValues)[key1] = keyValue1;
3094  if( !haveKey2 ) (*rootData.boundaryValues)[key2] = keyValue2;
3095  }
3096  }
3097  else
3098  {
3099  if( !haveKey1 ) rootData.cornerNormals[iter1] = keyValue1.second , rootData.cornerNormalsSet[ iter1 ] = 1;
3100  if( !haveKey2 ) rootData.cornerNormals[iter2] = keyValue2.second , rootData.cornerNormalsSet[ iter2 ] = 1;
3101  }
3102  }
3103 
3104  Point3D< Real > c;
3105  ri.node->centerAndWidth(c,width);
3106  center=c[o];
3107  for( i=0 ; i<DIMENSION ; i++ ) n[0][i] *= width , n[1][i] *= width;
3108 
3109  switch(o)
3110  {
3111  case 0:
3112  position[1] = c[1]-width/2+width*i1;
3113  position[2] = c[2]-width/2+width*i2;
3114  break;
3115  case 1:
3116  position[0] = c[0]-width/2+width*i1;
3117  position[2] = c[2]-width/2+width*i2;
3118  break;
3119  case 2:
3120  position[0] = c[0]-width/2+width*i1;
3121  position[1] = c[1]-width/2+width*i2;
3122  break;
3123  }
3124  double dx0,dx1;
3125  dx0 = n[0][o];
3126  dx1 = n[1][o];
3127 
3128  // The scaling will turn the Hermite Spline into a quadratic
3129  double scl=(x1-x0)/((dx1+dx0)/2);
3130  dx0 *= scl;
3131  dx1 *= scl;
3132 
3133  // Hermite Spline
3134  P.coefficients[0] = x0;
3135  P.coefficients[1] = dx0;
3136  P.coefficients[2] = 3*(x1-x0)-dx1-2*dx0;
3137 
3138  P.getSolutions( isoValue , roots , EPSILON );
3139  for( i=0 ; i<int(roots.size()) ; i++ )
3140  if( roots[i]>=0 && roots[i]<=1 )
3141  {
3142  averageRoot += Real( roots[i] );
3143  rCount++;
3144  }
3145  if( rCount && nonLinearFit ) averageRoot /= rCount;
3146  else averageRoot = Real((x0-isoValue)/(x0-x1));
3147  if( averageRoot<0 || averageRoot>1 )
3148  {
3149  fprintf( stderr , "[WARNING] Bad average root: %f\n" , averageRoot );
3150  fprintf( stderr , "\t(%f %f) , (%f %f) (%f)\n" , x0 , x1 , dx0 , dx1 , isoValue );
3151  if( averageRoot<0 ) averageRoot = 0;
3152  if( averageRoot>1 ) averageRoot = 1;
3153  }
3154  position[o] = Real(center-width/2+width*averageRoot);
3155  return 1;
3156  }
3157  template< int Degree >
3158  int Octree< Degree >::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , int sDepth,RootInfo& ri )
3159  {
3160  int c1,c2,f1,f2;
3161  const TreeOctNode *temp,*finest;
3162  int finestIndex;
3163 
3164  Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);
3165 
3166  finest=node;
3167  finestIndex=edgeIndex;
3168  if(node->depth()<maxDepth){
3169  if(IsBoundaryFace(node,f1,sDepth)){temp=NULL;}
3170  else{temp=node->faceNeighbor(f1);}
3171  if(temp && temp->children){
3172  finest=temp;
3173  finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1);
3174  }
3175  else{
3176  if(IsBoundaryFace(node,f2,sDepth)){temp=NULL;}
3177  else{temp=node->faceNeighbor(f2);}
3178  if(temp && temp->children){
3179  finest=temp;
3180  finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2);
3181  }
3182  else{
3183  if(IsBoundaryEdge(node,edgeIndex,sDepth)){temp=NULL;}
3184  else{temp=node->edgeNeighbor(edgeIndex);}
3185  if(temp && temp->children){
3186  finest=temp;
3187  finestIndex=Cube::EdgeReflectEdgeIndex(edgeIndex);
3188  }
3189  }
3190  }
3191  }
3192 
3193  Cube::EdgeCorners(finestIndex,c1,c2);
3194  if(finest->children){
3195  if (GetRootIndex(&finest->children[c1],finestIndex,maxDepth,sDepth,ri)) {return 1;}
3196  else if (GetRootIndex(&finest->children[c2],finestIndex,maxDepth,sDepth,ri)) {return 1;}
3197  else
3198  {
3199  fprintf( stderr , "[WARNING] Couldn't find root index with either child\n" );
3200  return 0;
3201  }
3202  }
3203  else
3204  {
3205  if( !(MarchingCubes::edgeMask()[finest->nodeData.mcIndex] & (1<<finestIndex)) )
3206  {
3207  fprintf( stderr , "[WARNING] Finest node does not have iso-edge\n" );
3208  return 0;
3209  }
3210 
3211  int o,i1,i2;
3212  Cube::FactorEdgeIndex(finestIndex,o,i1,i2);
3213  int d,off[3];
3214  finest->depthAndOffset(d,off);
3215  ri.node=finest;
3216  ri.edgeIndex=finestIndex;
3217  int eIndex[2],offset;
3218  offset=BinaryNode<Real>::Index( d , off[o] );
3219  switch(o)
3220  {
3221  case 0:
3222  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
3223  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3224  break;
3225  case 1:
3226  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3227  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3228  break;
3229  case 2:
3230  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3231  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
3232  break;
3233  }
3234  ri.key = (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45;
3235  return 1;
3236  }
3237  }
3238  template<int Degree>
3239  int Octree<Degree>::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , RootInfo& ri )
3240  {
3241  int c1,c2,f1,f2;
3242  const TreeOctNode *temp,*finest;
3243  int finestIndex;
3244 
3245 
3246  // The assumption is that the super-edge has a root along it.
3247  if(!(MarchingCubes::edgeMask()[node->nodeData.mcIndex] & (1<<edgeIndex))){return 0;}
3248 
3249  Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);
3250 
3251  finest = node;
3252  finestIndex = edgeIndex;
3253  if( node->depth()<maxDepth && !node->children )
3254  {
3255  temp=node->faceNeighbor( f1 );
3256  if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f1 );
3257  else
3258  {
3259  temp = node->faceNeighbor( f2 );
3260  if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f2 );
3261  else
3262  {
3263  temp = node->edgeNeighbor( edgeIndex );
3264  if( temp && temp->children ) finest = temp , finestIndex = Cube::EdgeReflectEdgeIndex( edgeIndex );
3265  }
3266  }
3267  }
3268 
3269  Cube::EdgeCorners( finestIndex , c1 , c2 );
3270  if( finest->children )
3271  {
3272  if ( GetRootIndex( finest->children + c1 , finestIndex , maxDepth , ri ) ) return 1;
3273  else if( GetRootIndex( finest->children + c2 , finestIndex , maxDepth , ri ) ) return 1;
3274  else
3275  {
3276  int d1 , off1[3] , d2 , off2[3];
3277  node->depthAndOffset( d1 , off1 );
3278  finest->depthAndOffset( d2 , off2 );
3279  fprintf( stderr , "[WARNING] Couldn't find root index with either child [%d] (%d %d %d) -> [%d] (%d %d %d) (%d %d)\n" , d1 , off1[0] , off1[1] , off1[2] , d2 , off2[0] , off2[1] , off2[2] , node->children!=NULL , finest->children!=NULL );
3280  printf( "\t" );
3281  for( int i=0 ; i<8 ; i++ ) if( node->nodeData.mcIndex & (1<<i) ) printf( "1" ); else printf( "0" );
3282  printf( "\t" );
3283  for( int i=0 ; i<8 ; i++ ) if( finest->nodeData.mcIndex & (1<<i) ) printf( "1" ); else printf( "0" );
3284  printf( "\n" );
3285  return 0;
3286  }
3287  }
3288  else
3289  {
3290  int o,i1,i2;
3291  Cube::FactorEdgeIndex(finestIndex,o,i1,i2);
3292  int d,off[3];
3293  finest->depthAndOffset(d,off);
3294  ri.node=finest;
3295  ri.edgeIndex=finestIndex;
3296  int offset,eIndex[2];
3297  offset = BinaryNode< Real >::CenterIndex( d , off[o] );
3298  switch(o){
3299  case 0:
3300  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
3301  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3302  break;
3303  case 1:
3304  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3305  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3306  break;
3307  case 2:
3308  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3309  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
3310  break;
3311  }
3312  ri.key= (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45;
3313  return 1;
3314  }
3315  }
3316  template<int Degree>
3317  int Octree<Degree>::GetRootPair(const RootInfo& ri,int maxDepth,RootInfo& pair){
3318  const TreeOctNode* node=ri.node;
3319  int c1,c2,c;
3320  Cube::EdgeCorners(ri.edgeIndex,c1,c2);
3321  while(node->parent){
3322  c=int(node-node->parent->children);
3323  if(c!=c1 && c!=c2){return 0;}
3324  if(!MarchingCubes::HasEdgeRoots(node->parent->nodeData.mcIndex,ri.edgeIndex)){
3325  if(c==c1){return GetRootIndex(&node->parent->children[c2],ri.edgeIndex,maxDepth,pair);}
3326  else{return GetRootIndex(&node->parent->children[c1],ri.edgeIndex,maxDepth,pair);}
3327  }
3328  node=node->parent;
3329  }
3330  return 0;
3331 
3332  }
3333  template<int Degree>
3334  int Octree< Degree >::GetRootIndex( const RootInfo& ri , RootData& rootData , CoredPointIndex& index )
3335  {
3336  long long key = ri.key;
3337  auto rootIter = rootData.boundaryRoots.find( key );
3338  if( rootIter!=rootData.boundaryRoots.end() )
3339  {
3340  index.inCore = 1;
3341  index.index = rootIter->second;
3342  return 1;
3343  }
3344  else if( rootData.interiorRoots )
3345  {
3346  int eIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ];
3347  if( rootData.edgesSet[ eIndex ] )
3348  {
3349  index.inCore = 0;
3350  index.index = rootData.interiorRoots[ eIndex ];
3351  return 1;
3352  }
3353  }
3354  return 0;
3355  }
3356  template< int Degree >
3357  int Octree< Degree >::SetMCRootPositions( TreeOctNode* node , int sDepth , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , RootData& rootData ,
3358  std::vector< Point3D< float > >* interiorPositions , CoredMeshData* mesh , const Real* metSolution , int nonLinearFit )
3359  {
3360  Point3D< Real > position;
3361  int eIndex;
3362  RootInfo ri;
3363  int count=0;
3364  if( !MarchingCubes::HasRoots( node->nodeData.mcIndex ) ) return 0;
3365  for( int i=0 ; i<DIMENSION ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
3366  {
3367  long long key;
3368  eIndex = Cube::EdgeIndex( i , j , k );
3369  if( GetRootIndex( node , eIndex , fData.depth , ri ) )
3370  {
3371  key = ri.key;
3372  if( !rootData.interiorRoots || IsBoundaryEdge( node , i , j , k , sDepth ) )
3373  {
3374  std::unordered_map< long long , int >::iterator iter , end;
3375  // Check if the root has already been set
3376 #pragma omp critical (boundary_roots_hash_access)
3377  {
3378  iter = rootData.boundaryRoots.find( key );
3379  end = rootData.boundaryRoots.end();
3380  }
3381  if( iter==end )
3382  {
3383  // Get the root information
3384  GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit );
3385  // Add the root if it hasn't been added already
3386 #pragma omp critical (boundary_roots_hash_access)
3387  {
3388  iter = rootData.boundaryRoots.find( key );
3389  end = rootData.boundaryRoots.end();
3390  if( iter==end )
3391  {
3392  mesh->inCorePoints.push_back( position );
3393  rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() ) - 1;
3394  }
3395  }
3396  if( iter==end ) count++;
3397  }
3398  }
3399  else
3400  {
3401  int nodeEdgeIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ];
3402  if( !rootData.edgesSet[ nodeEdgeIndex ] )
3403  {
3404  // Get the root information
3405  GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit );
3406  // Add the root if it hasn't been added already
3407 #pragma omp critical (add_point_access)
3408  {
3409  if( !rootData.edgesSet[ nodeEdgeIndex ] )
3410  {
3411  rootData.interiorRoots[ nodeEdgeIndex ] = mesh->addOutOfCorePoint( position );
3412  interiorPositions->push_back( position );
3413  rootData.edgesSet[ nodeEdgeIndex ] = 1;
3414  count++;
3415  }
3416  }
3417  }
3418  }
3419  }
3420  }
3421  return count;
3422  }
3423  template<int Degree>
3424  int Octree< Degree >::SetBoundaryMCRootPositions( int sDepth , Real isoValue , RootData& rootData , CoredMeshData* mesh , int nonLinearFit )
3425  {
3426  Point3D< Real > position;
3427  int i,j,k,eIndex,hits=0;
3428  RootInfo ri;
3429  int count=0;
3430  TreeOctNode* node;
3431 
3432  node = tree.nextLeaf();
3433  while( node )
3434  {
3436  {
3437  hits=0;
3438  for( i=0 ; i<DIMENSION ; i++ )
3439  for( j=0 ; j<2 ; j++ )
3440  for( k=0 ; k<2 ; k++ )
3441  if( IsBoundaryEdge( node , i , j , k , sDepth ) )
3442  {
3443  hits++;
3444  long long key;
3445  eIndex = Cube::EdgeIndex( i , j , k );
3446  if( GetRootIndex( node , eIndex , fData.depth , ri ) )
3447  {
3448  key = ri.key;
3449  if( rootData.boundaryRoots.find(key)==rootData.boundaryRoots.end() )
3450  {
3451  GetRoot( ri , isoValue , position , rootData , sDepth , nonLinearFit );
3452  mesh->inCorePoints.push_back( position );
3453  rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() )-1;
3454  count++;
3455  }
3456  }
3457  }
3458  }
3459  if( hits ) node=tree.nextLeaf(node);
3460  else node=tree.nextBranch(node);
3461  }
3462  return count;
3463  }
3464  template<int Degree>
3465  void Octree< Degree >::GetMCIsoEdges( TreeOctNode* node , int sDepth , std::vector< std::pair< RootInfo , RootInfo > >& edges )
3466  {
3467  TreeOctNode* temp;
3468  int count=0 , tris=0;
3469  int isoTri[ DIMENSION * MarchingCubes::MAX_TRIANGLES ];
3470  FaceEdgesFunction fef;
3471  int ref , fIndex;
3472  std::unordered_map< long long , std::pair< RootInfo , int > > vertexCount;
3473 
3474  fef.edges = &edges;
3475  fef.maxDepth = fData.depth;
3476  fef.vertexCount = &vertexCount;
3477  count = MarchingCubes::AddTriangleIndices( node->nodeData.mcIndex , isoTri );
3478  for( fIndex=0 ; fIndex<Cube::NEIGHBORS ; fIndex++ )
3479  {
3480  ref = Cube::FaceReflectFaceIndex( fIndex , fIndex );
3481  fef.fIndex = ref;
3482  temp = node->faceNeighbor( fIndex );
3483  // If the face neighbor exists and has higher resolution than the current node,
3484  // get the iso-curve from the neighbor
3485  if( temp && temp->children && !IsBoundaryFace( node , fIndex , sDepth ) ) temp->processNodeFaces( temp , &fef , ref );
3486  // Otherwise, get it from the node
3487  else
3488  {
3489  RootInfo ri1 , ri2;
3490  for( int j=0 ; j<count ; j++ )
3491  for( int k=0 ; k<3 ; k++ )
3492  if( fIndex==Cube::FaceAdjacentToEdges( isoTri[j*3+k] , isoTri[j*3+((k+1)%3)] ) )
3493  if( GetRootIndex( node , isoTri[j*3+k] , fData.depth , ri1 ) && GetRootIndex( node , isoTri[j*3+((k+1)%3)] , fData.depth , ri2 ) )
3494  {
3495  long long key1 = ri1.key , key2 = ri2.key;
3496  edges.push_back( std::pair< RootInfo , RootInfo >( ri1 , ri2 ) );
3497  if( vertexCount.count( key1 )==0 )
3498  {
3499  vertexCount[key1].first = ri1;
3500  vertexCount[key1].second = 0;
3501  }
3502  if( vertexCount.count( key2 )==0 )
3503  {
3504  vertexCount[key2].first = ri2;
3505  vertexCount[key2].second = 0;
3506  }
3507  vertexCount[key1].second++;
3508  vertexCount[key2].second--;
3509  }
3510  else
3511  {
3512  int r1 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+k] );
3513  int r2 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+((k+1)%3)] );
3514  fprintf( stderr , "Bad Edge 2: %d %d\t%d %d\n" , ri1.key , ri2.key , r1 , r2 );
3515  }
3516  }
3517  }
3518  for( int i=0 ; i<int(edges.size()) ; i++ )
3519  {
3520  if( vertexCount.count( edges[i].first.key )==0 ) printf( "Could not find vertex: %lld\n" , edges[i].first );
3521  else if( vertexCount[ edges[i].first.key ].second )
3522  {
3523  RootInfo ri;
3524  GetRootPair( vertexCount[edges[i].first.key].first , fData.depth , ri );
3525  long long key = ri.key;
3526  if( vertexCount.count( key )==0 )
3527  {
3528  int d , off[3];
3529  node->depthAndOffset( d , off );
3530  printf( "Vertex pair not in list 1 (%lld) %d\t[%d] (%d %d %d)\n" , key , IsBoundaryEdge( ri.node , ri.edgeIndex , sDepth ) , d , off[0] , off[1] , off[2] );
3531  }
3532  else
3533  {
3534  edges.push_back( std::pair< RootInfo , RootInfo >( ri , edges[i].first ) );
3535  vertexCount[ key ].second++;
3536  vertexCount[ edges[i].first.key ].second--;
3537  }
3538  }
3539 
3540  if( vertexCount.count( edges[i].second.key )==0 ) printf( "Could not find vertex: %lld\n" , edges[i].second );
3541  else if( vertexCount[edges[i].second.key].second )
3542  {
3543  RootInfo ri;
3544  GetRootPair( vertexCount[edges[i].second.key].first , fData.depth , ri );
3545  long long key = ri.key;
3546  if( vertexCount.count( key ) )
3547  {
3548  int d , off[3];
3549  node->depthAndOffset( d , off );
3550  printf( "Vertex pair not in list 2\t[%d] (%d %d %d)\n" , d , off[0] , off[1] , off[2] );
3551  }
3552  else
3553  {
3554  edges.push_back( std::pair< RootInfo , RootInfo >( edges[i].second , ri ) );
3555  vertexCount[key].second--;
3556  vertexCount[ edges[i].second.key ].second++;
3557  }
3558  }
3559  }
3560  }
3561  template<int Degree>
3562 #if MISHA_DEBUG
3563  int Octree< Degree >::GetMCIsoTriangles( TreeOctNode* node , CoredMeshData* mesh , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , int offSet , int sDepth , bool polygonMesh , std::vector< Point3D< float > >* barycenters )
3564 #else // !MISHA_DEBUG
3565  int Octree< Degree >::GetMCIsoTriangles( TreeOctNode* node , CoredMeshData* mesh , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , int offSet , int sDepth , bool addBarycenter , bool polygonMesh )
3566 #endif // MISHA_DEBUG
3567  {
3568  int tris=0;
3569  std::vector< std::pair< RootInfo , RootInfo > > edges;
3570  std::vector< std::vector< std::pair< RootInfo , RootInfo > > > edgeLoops;
3571  GetMCIsoEdges( node , sDepth , edges );
3572 
3573  GetEdgeLoops( edges , edgeLoops );
3574  for( int i=0 ; i<int(edgeLoops.size()) ; i++ )
3575  {
3576  CoredPointIndex p;
3577  std::vector<CoredPointIndex> edgeIndices;
3578  for( int j=0 ; j<int(edgeLoops[i].size()) ; j++ )
3579  {
3580  if( !GetRootIndex( edgeLoops[i][j].first , rootData , p ) ) printf( "Bad Point Index\n" );
3581  else edgeIndices.push_back( p );
3582  }
3583 #if MISHA_DEBUG
3584  tris += AddTriangles( mesh , edgeIndices , interiorPositions , offSet , polygonMesh , barycenters );
3585 #else // !MISHA_DEBUG
3586  tris += AddTriangles( mesh , edgeIndices , interiorPositions , offSet , addBarycenter , polygonMesh );
3587 #endif // MISHA_DEBUG
3588  }
3589  return tris;
3590  }
3591 
3592  template< int Degree >
3593  int Octree< Degree >::GetEdgeLoops( std::vector< std::pair< RootInfo , RootInfo > >& edges , std::vector< std::vector< std::pair< RootInfo , RootInfo > > >& loops )
3594  {
3595  int loopSize=0;
3596  long long frontIdx , backIdx;
3597  std::pair< RootInfo , RootInfo > e , temp;
3598  loops.clear();
3599 
3600  while( edges.size() )
3601  {
3602  std::vector< std::pair< RootInfo , RootInfo > > front , back;
3603  e = edges[0];
3604  loops.resize( loopSize+1 );
3605  edges[0] = edges.back();
3606  edges.pop_back();
3607  frontIdx = e.second.key;
3608  backIdx = e.first.key;
3609  for( int j=int(edges.size())-1 ; j>=0 ; j-- )
3610  {
3611  if( edges[j].first.key==frontIdx || edges[j].second.key==frontIdx )
3612  {
3613  if( edges[j].first.key==frontIdx ) temp = edges[j];
3614  else temp.first = edges[j].second , temp.second = edges[j].first;
3615  frontIdx = temp.second.key;
3616  front.push_back(temp);
3617  edges[j] = edges.back();
3618  edges.pop_back();
3619  j = int(edges.size());
3620  }
3621  else if( edges[j].first.key==backIdx || edges[j].second.key==backIdx )
3622  {
3623  if( edges[j].second.key==backIdx ) temp = edges[j];
3624  else temp.first = edges[j].second , temp.second = edges[j].first;
3625  backIdx = temp.first.key;
3626  back.push_back(temp);
3627  edges[j] = edges.back();
3628  edges.pop_back();
3629  j = int(edges.size());
3630  }
3631  }
3632  for( int j=int(back.size())-1 ; j>=0 ; j-- ) loops[loopSize].push_back( back[j] );
3633  loops[loopSize].push_back(e);
3634  for( int j=0 ; j<int(front.size()) ; j++ ) loops[loopSize].push_back( front[j] );
3635  loopSize++;
3636  }
3637  return int(loops.size());
3638  }
3639  template<int Degree>
3640 #if MISHA_DEBUG
3641  int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool polygonMesh , std::vector< Point3D< float > >* barycenters )
3642 #else // !MISHA_DEBUG
3643  int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool addBarycenter , bool polygonMesh )
3644 #endif // MISHA_DEBUG
3645  {
3647  std::vector< Point3D< float > > vertices;
3648  std::vector< TriangleIndex > triangles;
3649  if( polygonMesh )
3650  {
3651  std::vector< CoredVertexIndex > vertices( edges.size() );
3652  for( int i=0 ; i<int(edges.size()) ; i++ )
3653  {
3654  vertices[i].idx = edges[i].index;
3655  vertices[i].inCore = (edges[i].inCore!=0);
3656  }
3657 #pragma omp critical (add_polygon_access)
3658  {
3659  mesh->addPolygon( vertices );
3660  }
3661  return 1;
3662  }
3663  if( edges.size()>3 )
3664  {
3665  bool isCoplanar = false;
3666 
3667 #if MISHA_DEBUG
3668  if( barycenters )
3669 #else // !MISHA_DEBUG
3670  if( addBarycenter )
3671 #endif // MISHA_DEBUG
3672  for( int i=0 ; i<int(edges.size()) ; i++ )
3673  for( int j=0 ; j<i ; j++ )
3674  if( (i+1)%edges.size()!=j && (j+1)%edges.size()!=i )
3675  {
3676  Point3D< Real > v1 , v2;
3677  if( edges[i].inCore ) v1 = mesh->inCorePoints[ edges[i].index ];
3678  else v1 = (*interiorPositions)[ edges[i].index-offSet ];
3679  if( edges[j].inCore ) v2 = mesh->inCorePoints[ edges[j].index ];
3680  else v2 = (*interiorPositions)[ edges[j].index-offSet ];
3681  for( int k=0 ; k<3 ; k++ ) if( v1[k]==v2[k] ) isCoplanar = true;
3682  }
3683  if( isCoplanar )
3684  {
3685  Point3D< Real > c;
3686  c[0] = c[1] = c[2] = 0;
3687  for( int i=0 ; i<int(edges.size()) ; i++ )
3688  {
3689  Point3D<Real> p;
3690  if(edges[i].inCore) p = mesh->inCorePoints[edges[i].index ];
3691  else p = (*interiorPositions)[edges[i].index-offSet];
3692  c += p;
3693  }
3694  c /= Real( edges.size() );
3695  int cIdx;
3696 #pragma omp critical (add_point_access)
3697  {
3698  cIdx = mesh->addOutOfCorePoint( c );
3699 #if MISHA_DEBUG
3700  barycenters->push_back( c );
3701 #else // !MISHA_DEBUG
3702  interiorPositions->push_back( c );
3703 #endif // MISHA_DEBUG
3704  }
3705  for( int i=0 ; i<int(edges.size()) ; i++ )
3706  {
3707  std::vector< CoredVertexIndex > vertices( 3 );
3708  vertices[0].idx = edges[i ].index;
3709  vertices[1].idx = edges[(i+1)%edges.size()].index;
3710  vertices[2].idx = cIdx;
3711  vertices[0].inCore = (edges[i ].inCore!=0);
3712  vertices[1].inCore = (edges[(i+1)%edges.size()].inCore!=0);
3713  vertices[2].inCore = 0;
3714 #pragma omp critical (add_polygon_access)
3715  {
3716  mesh->addPolygon( vertices );
3717  }
3718  }
3719  return int( edges.size() );
3720  }
3721  else
3722  {
3723  vertices.resize( edges.size() );
3724  // Add the points
3725  for( int i=0 ; i<int(edges.size()) ; i++ )
3726  {
3727  Point3D< Real > p;
3728  if( edges[i].inCore ) p = mesh->inCorePoints[edges[i].index ];
3729  else p = (*interiorPositions)[edges[i].index-offSet];
3730  vertices[i] = p;
3731  }
3732  MAT.GetTriangulation( vertices , triangles );
3733  for( int i=0 ; i<int(triangles.size()) ; i++ )
3734  {
3735  std::vector< CoredVertexIndex > _vertices( 3 );
3736  for( int j=0 ; j<3 ; j++ )
3737  {
3738  _vertices[j].idx = edges[ triangles[i].idx[j] ].index;
3739  _vertices[j].inCore = (edges[ triangles[i].idx[j] ].inCore!=0);
3740  }
3741 #pragma omp critical (add_polygon_access)
3742  {
3743  mesh->addPolygon( _vertices );
3744  }
3745  }
3746  }
3747  }
3748  else if( edges.size()==3 )
3749  {
3750  std::vector< CoredVertexIndex > vertices( 3 );
3751  for( int i=0 ; i<3 ; i++ )
3752  {
3753  vertices[i].idx = edges[i].index;
3754  vertices[i].inCore = (edges[i].inCore!=0);
3755  }
3756 #pragma omp critical (add_polygon_access)
3757  mesh->addPolygon( vertices );
3758  }
3759  return int(edges.size())-2;
3760  }
3761  template< int Degree >
3762  Real* Octree< Degree >::GetSolutionGrid( int& res , float isoValue , int depth )
3763  {
3764  if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth();
3766  fData.set( depth );
3767  fData.setValueTables( fData.VALUE_FLAG );
3768  res = 1<<depth;
3769  Real* values = new float[ res * res * res ];
3770  memset( values , 0 , sizeof( float ) * res * res * res );
3771 
3772  for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode( n ) )
3773  {
3774  if( n->d>depth ) continue;
3775  if( n->d<_minDepth ) continue;
3776  int d , idx[3] , start[3] , end[3];
3777  n->depthAndOffset( d , idx );
3778  for( int i=0 ; i<3 ; i++ )
3779  {
3780  // Get the index of the functions
3781  idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] );
3782  // Figure out which samples fall into the range
3783  fData.setSampleSpan( idx[i] , start[i] , end[i] );
3784  // We only care about the odd indices
3785  if( !(start[i]&1) ) start[i]++;
3786  if( !( end[i]&1) ) end[i]--;
3787  }
3788  Real coefficient = n->nodeData.solution;
3789  for( int x=start[0] ; x<=end[0] ; x+=2 )
3790  for( int y=start[1] ; y<=end[1] ; y+=2 )
3791  for( int z=start[2] ; z<=end[2] ; z+=2 )
3792  {
3793  int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1;
3794  values[ zz*res*res + yy*res + xx ] +=
3795  coefficient *
3796  fData.valueTables[ idx[0]+x*fData.functionCount ] *
3797  fData.valueTables[ idx[1]+y*fData.functionCount ] *
3798  fData.valueTables[ idx[2]+z*fData.functionCount ];
3799  }
3800  }
3801  for( int i=0 ; i<res*res*res ; i++ ) values[i] -= isoValue;
3802 
3803  return values;
3804  }
3805  template< int Degree >
3806  Real* Octree< Degree >::GetWeightGrid( int& res , int depth )
3807  {
3808  if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth();
3809  res = 1<<tree.maxDepth();
3810  Real* values = new float[ res * res * res ];
3811  memset( values , 0 , sizeof( float ) * res * res * res );
3812 
3813  for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode( n ) )
3814  {
3815  if( n->d>depth ) continue;
3816  int d , idx[3] , start[3] , end[3];
3817  n->depthAndOffset( d , idx );
3818  for( int i=0 ; i<3 ; i++ )
3819  {
3820  // Get the index of the functions
3821  idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] );
3822  // Figure out which samples fall into the range
3823  fData.setSampleSpan( idx[i] , start[i] , end[i] );
3824  // We only care about the odd indices
3825  if( !(start[i]&1) ) start[i]++;
3826  if( !( end[i]&1) ) end[i]--;
3827  }
3828  for( int x=start[0] ; x<=end[0] ; x+=2 )
3829  for( int y=start[1] ; y<=end[1] ; y+=2 )
3830  for( int z=start[2] ; z<=end[2] ; z+=2 )
3831  {
3832  int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1;
3833  values[ zz*res*res + yy*res + xx ] +=
3834  n->nodeData.centerWeightContribution *
3835  fData.valueTables[ idx[0]+x*fData.functionCount ] *
3836  fData.valueTables[ idx[1]+y*fData.functionCount ] *
3837  fData.valueTables[ idx[2]+z*fData.functionCount ];
3838  }
3839  }
3840  return values;
3841  }
3842 
3843  ////////////////
3844  // VertexData //
3845  ////////////////
3846  long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth){
3847  int idx[DIMENSION];
3848  return CenterIndex(node,maxDepth,idx);
3849  }
3850  long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth,int idx[DIMENSION]){
3851  int d,o[3];
3852  node->depthAndOffset(d,o);
3853  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
3854  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3855  }
3856  long long VertexData::CenterIndex(int depth,const int offSet[DIMENSION],int maxDepth,int idx[DIMENSION]){
3857  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,depth+1,offSet[i]<<1,1);}
3858  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3859  }
3860  long long VertexData::CornerIndex(const TreeOctNode* node,int cIndex,int maxDepth){
3861  int idx[DIMENSION];
3862  return CornerIndex(node,cIndex,maxDepth,idx);
3863  }
3864  long long VertexData::CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int idx[DIMENSION] )
3865  {
3866  int x[DIMENSION];
3867  Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] );
3868  int d , o[3];
3869  node->depthAndOffset( d , o );
3870  for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , d , o[i] , x[i] );
3871  return CornerIndexKey( idx );
3872  }
3873  long long VertexData::CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int idx[DIMENSION] )
3874  {
3875  int x[DIMENSION];
3876  Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] );
3877  for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , depth , offSet[i] , x[i] );
3878  return CornerIndexKey( idx );
3879  }
3880  long long VertexData::CornerIndexKey( const int idx[DIMENSION] )
3881  {
3882  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3883  }
3884  long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth){
3885  int idx[DIMENSION];
3886  return FaceIndex(node,fIndex,maxDepth,idx);
3887  }
3888  long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth,int idx[DIMENSION]){
3889  int dir,offset;
3890  Cube::FactorFaceIndex(fIndex,dir,offset);
3891  int d,o[3];
3892  node->depthAndOffset(d,o);
3893  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
3894  idx[dir]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,o[dir],offset);
3895  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3896  }
3897  long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth){
3898  int idx[DIMENSION];
3899  return EdgeIndex(node,eIndex,maxDepth,idx);
3900  }
3901  long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth,int idx[DIMENSION]){
3902  int o,i1,i2;
3903  int d,off[3];
3904  node->depthAndOffset(d,off);
3905  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,off[i]<<1,1);}
3906  Cube::FactorEdgeIndex(eIndex,o,i1,i2);
3907  switch(o){
3908  case 0:
3909  idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
3910  idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3911  break;
3912  case 1:
3913  idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3914  idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3915  break;
3916  case 2:
3917  idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3918  idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
3919  break;
3920  };
3921  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3922  }
3923 
3924 
3925  }
3926 }
3927 
3928 
UpSampleData(int s, double v1, double v2)
CornerIndices & cornerIndices(const TreeOctNode *node)
CornerIndices & operator[](const TreeOctNode *node)
EdgeIndices & operator[](const TreeOctNode *node)
int getMaxCornerCount(const TreeOctNode *rootNode, int depth, int maxDepth, int threads) const
double coefficients[Degree+1]
Definition: polynomial.h:42
static void FactorCornerIndex(int idx, int &x, int &y, int &z)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
virtual int addOutOfCorePoint(const Point3D< float > &p)=0
static int AddTriangleIndices(int mcIndex, int *triangles)
static long long FaceIndex(const TreeOctNode *node, int fIndex, int maxDepth, int index[DIMENSION])
void RefineBoundary(int subdivisionDepth)
double Length(const Point3D< Real > &p)
Definition: geometry.hpp:65
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void getSolutions(double c, std::vector< double > &roots, double EPS) const
Definition: polynomial.hpp:272
OctNode * edgeNeighbor(int edgeIndex, int forceChildren=0)
Definition: sparse_matrix.h:45
static int SymmetricIndex(int i1, int i2)
Real * GetWeightGrid(int &res, int depth=-1)
void SetRowSize(int row, int count)
static int EdgeReflectEdgeIndex(int edgeIndex)
static void FacesAdjacentToEdge(int eIndex, int &f1Index, int &f2Index)
const OctNode * nextBranch(const OctNode *current) const
static int CornerIndex(int x, int y)
EdgeIndices & edgeIndices(const TreeOctNode *node)
T Value
Definition: sparse_matrix.h:50
int N
Definition: sparse_matrix.h:49
const OctNode * neighbors[3][3][3]
int LaplacianMatrixIteration(int subdivideDepth, bool showResidual, int minIters, double accuracy)
OctNode * faceNeighbor(int faceIndex, int forceChildren=0)
static void FaceCorners(int idx, int &c1, int &c2, int &c3, int &c4)
static int CornerIndex(const Point3D< Real > &center, const Point3D< Real > &p)
static int HasRoots(const double v[Cube::CORNERS], double isoValue)
static int FaceReflectEdgeIndex(int idx, int faceIndex)
An exception that is thrown when something goes wrong inside an openMP for loop.
void processNodeFaces(OctNode *node, NodeAdjacencyFunction *F, int fIndex, int processCurrent=1)
static void FactorEdgeIndex(int idx, int &orientation, int &i, int &j)
void centerAndWidth(Point3D< Real > &center, Real &width) const
static long long CornerIndexKey(const int index[DIMENSION])
void resize(int threads, int dim)
T Norm(size_t Ln) const
Definition: vector.hpp:214
void setFullDepth(int maxDepth)
void atomicOr(volatile int &dest, int value)
void Resize(size_t N)
Definition: vector.hpp:63
static void Index(int depth, const int offset[3], short &d, short off[DIMENSION])
static void EdgeCorners(int idx, int &c1, int &c2)
const OctNode * nextLeaf(const OctNode *currentLeaf=NULL) const
static int HasEdgeRoots(int mcIndex, int edgeIndex)
virtual void setValueTables(int flags, double smooth=0)
static void ProcessFixedDepthNodeAdjacentNodes(int maxDepth, OctNode *node1, int width1, OctNode *node2, int width2, int depth, NodeAdjacencyFunction *F, int processCurrent=1)
static int GetIndex(const double values[Cube::CORNERS], double iso)
static int Solve(const SparseSymmetricMatrix< T > &M, const Vector< T2 > &b, int iters, Vector< T2 > &solution, T2 eps=1e-8, int reset=1, int threads=0, bool addDCTerm=false, bool solveNormal=false)
static void FactorCornerIndex(int idx, int &x, int &y)
static int CornerIndex(int depth, int offSet)
Definition: binary_node.h:48
static long long CenterIndex(int depth, const int offSet[DIMENSION], int maxDepth, int index[DIMENSION])
short off[DIMENSION]
static const int * cornerMap()
static void SetAllocator(int blockSize)
static int Index(int depth, int offSet)
Definition: binary_node.h:46
int maxDepth(void) const
static long long CornerIndex(int depth, const int offSet[DIMENSION], int cIndex, int maxDepth, int index[DIMENSION])
static int FaceAdjacentToEdges(int eIndex1, int eIndex2)
static int AntipodalCornerIndex(int idx)
static double MemoryUsage(void)
virtual int addPolygon(const std::vector< CoredVertexIndex > &vertices)=0
static int AntipodalCornerIndex(int idx)
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:442
int getMaxEdgeCount(const TreeOctNode *rootNode, int depth, int threads) const
void setCornerTable(CornerTableData &cData, const TreeOctNode *rootNode, int depth, int threads) const
static int CornerIndex(int x, int y, int z)
void set(int maxDepth, bool useDotRatios=true, bool reflectBoundary=false)
static int CenterIndex(int depth, int offSet)
Definition: binary_node.h:47
void setEdgeTable(EdgeTableData &eData, const TreeOctNode *rootNode, int depth, int threads)
static int FaceReflectFaceIndex(int idx, int faceIndex)
virtual int outOfCorePointCount(void)=0
static long long EdgeIndex(const TreeOctNode *node, int eIndex, int maxDepth, int index[DIMENSION])
void depthAndOffset(int &depth, int offset[DIMENSION]) const
static int EdgeIndex(int orientation, int i, int j)
Definition: norms.h:54
Real * GetSolutionGrid(int &res, float isoValue=0.f, int depth=-1)
static const int * edgeMask()
static void FactorFaceIndex(int idx, int &x, int &y, int &z)
const OctNode * nextNode(const OctNode *currentNode=NULL) const
std::vector< Point3D< float > > inCorePoints
Definition: geometry.h:202
const Real MATRIX_ENTRY_EPSILON
void setSampleSpan(int idx, int &start, int &end, double smooth=0) const
int setTree(typename pcl::PointCloud< PointNT >::ConstPtr input_, int maxDepth, int minDepth, int kernelDepth, Real samplesPerNode, Real scaleFactor, Point3D< Real > &center, Real &scale, int useConfidence, Real constraintWeight, bool adaptiveWeights)
size_t size() const
Definition: point_cloud.h:461
void setBSplineData(int maxDepth, Real normalSmooth=-1, bool reflectBoundary=false)
static const int VALUE_FLAG
Definition: bspline_data.h:59