Point Cloud Library (PCL)  1.10.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 
1132  template< int Degree >
1133  int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const
1134  {
1135  int count = 0;
1136  for( int x=xStart ; x<=2 ; x++ )
1137  for( int y=yStart ; y<yEnd ; y++ )
1138  if( x==2 && y>2 ) continue;
1139  else for( int z=zStart ; z<zEnd ; z++ )
1140  if( x==2 && y==2 && z>2 ) continue;
1141  else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 )
1142  count++;
1143  return count;
1144  }
1145 
1146  template< int Degree >
1147  int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] ) const
1148  {
1149  return SetMatrixRow( neighbors5 , row , offset , stencil , 0 , 5 , 0 , 5 , 0 , 5 );
1150  }
1151 
1152  template< int Degree >
1153  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
1154  {
1155  bool hasPoints[3][3];
1156  Real diagonal = 0;
1157  PointInfo samples[3][3][3];
1158 
1159  int count = 0;
1160  const TreeOctNode* node = neighbors5.neighbors[2][2][2];
1161  int index[] = { int( node->off[0] ) , int( node->off[1] ), int( node->off[2] ) };
1162 
1163  if( _constrainValues )
1164  {
1165  int d , idx[3];
1166  node->depthAndOffset( d , idx );
1167  idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] );
1168  idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] );
1169  idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] );
1170  for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ )
1171  {
1172  hasPoints[j][k] = false;
1173  for( int l=0 ; l<3 ; l++ )
1174  {
1175  const TreeOctNode* _node = neighbors5.neighbors[j+1][k+1][l+1];
1176  if( _node && _node->nodeData.pointIndex!=-1 )
1177  {
1178  const PointData& pData = _points[ _node->nodeData.pointIndex ];
1179  PointInfo& pointInfo = samples[j][k][l];
1180  Real weight = pData.weight;
1181  Point3D< Real > p = pData.position;
1182  for( int s=0 ; s<3 ; s++ )
1183  {
1184  pointInfo.splineValues[0][s] = float( fData.baseBSplines[ idx[0]+j-s][s]( p[0] ) );
1185  pointInfo.splineValues[1][s] = float( fData.baseBSplines[ idx[1]+k-s][s]( p[1] ) );
1186  pointInfo.splineValues[2][s] = float( fData.baseBSplines[ idx[2]+l-s][s]( p[2] ) );
1187  }
1188  float value = pointInfo.splineValues[0][j] * pointInfo.splineValues[1][k] * pointInfo.splineValues[2][l];
1189  diagonal += value * value * weight;
1190  pointInfo.weightedValue = value * weight;
1191  for( int s=0 ; s<3 ; s++ ) pointInfo.splineValues[0][s] *= pointInfo.weightedValue;
1192  hasPoints[j][k] = true;
1193  }
1194  else samples[j][k][l].weightedValue = 0;
1195  }
1196  }
1197  }
1198 
1199  bool isInterior;
1200  int d , off[3];
1201  neighbors5.neighbors[2][2][2]->depthAndOffset( d , off );
1202  int mn = 2 , mx = (1<<d)-2;
1203  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
1204  for( int x=xStart ; x<=2 ; x++ )
1205  for( int y=yStart ; y<yEnd ; y++ )
1206  if( x==2 && y>2 ) continue;
1207  else for( int z=zStart ; z<zEnd ; z++ )
1208  if( x==2 && y==2 && z>2 ) continue;
1209  else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 )
1210  {
1211  const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
1212  int _index[] = { int( _node->off[0] ) , int( _node->off[1] ), int( _node->off[2] ) };
1213  Real temp;
1214  if( isInterior ) temp = Real( stencil[x][y][z] );
1215  else temp = GetLaplacian( node , _node );
1216  if( _constrainValues )
1217  {
1218  int _d[] = { _index[0]-index[0] , _index[1]-index[1] , _index[2]-index[2] };
1219  if( x==2 && y==2 && z==2 ) temp += diagonal;
1220  else temp += GetValue( samples , hasPoints , _d );
1221  }
1222  if( x==2 && y==2 && z==2 ) temp /= 2;
1223  if( fabs(temp)>MATRIX_ENTRY_EPSILON )
1224  {
1225  row[count].N = _node->nodeData.nodeIndex-offset;
1226  row[count].Value = temp;
1227  count++;
1228  }
1229  }
1230  return count;
1231  }
1232 
1233  template< int Degree >
1234  void Octree< Degree >::SetDivergenceStencil( int depth , Point3D< double > *stencil , bool scatter ) const
1235  {
1236  int offset[] = { 2 , 2 , 2 };
1237  short d , off[3];
1238  TreeOctNode::Index( depth , offset , d , off );
1239  int index1[3] , index2[3];
1240  if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
1241  else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
1242  for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
1243  {
1244  int _offset[] = { x , y , z };
1245  TreeOctNode::Index( depth , _offset , d , off );
1246  if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
1247  else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
1248  int symIndex[] =
1249  {
1250  BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) ,
1251  BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) ,
1252  BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) ,
1253  };
1254  int aSymIndex[] =
1255  {
1256  #if GRADIENT_DOMAIN_SOLUTION
1257  // Take the dot-product of the vector-field with the gradient of the basis function
1258  fData.Index( index1[0] , index2[0] ) ,
1259  fData.Index( index1[1] , index2[1] ) ,
1260  fData.Index( index1[2] , index2[2] )
1261  #else // !GRADIENT_DOMAIN_SOLUTION
1262  // Take the dot-product of the divergence of the vector-field with the basis function
1263  fData.Index( index2[0] , index1[0] ) ,
1264  fData.Index( index2[1] , index1[1] ) ,
1265  fData.Index( index2[2] , index1[2] )
1266  #endif // GRADIENT_DOMAIN_SOLUTION
1267  };
1268 
1269  double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
1270 #if GRADIENT_DOMAIN_SOLUTION
1271  Point3D<double> temp;
1272  temp[0] = fData.dvDotTable[aSymIndex[0]] * dot;
1273  temp[1] = fData.dvDotTable[aSymIndex[1]] * dot;
1274  temp[2] = fData.dvDotTable[aSymIndex[2]] * dot;
1275  stencil[25*x + 5*y + z] = temp;
1276  // stencil[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot;
1277  // stencil[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot;
1278  // stencil[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot;
1279 #else // !GRADIENT_DOMAIN_SOLUTION
1280  Point3D<double> temp;
1281  temp[0] = -fData.dvDotTable[aSymIndex[0]] * dot;
1282  temp[1] = -fData.dvDotTable[aSymIndex[1]] * dot;
1283  temp[2] = -fData.dvDotTable[aSymIndex[2]] * dot;
1284  stencil[25*x + 5*y + z] = temp;
1285  // stencil[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot;
1286  // stencil[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot;
1287  // stencil[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot;
1288 #endif // GRADIENT_DOMAIN_SOLUTION
1289  }
1290  }
1291 
1292  template< int Degree >
1293  void Octree< Degree >::SetLaplacianStencil( int depth , double stencil[5][5][5] ) const
1294  {
1295  int offset[] = { 2 , 2 , 2 };
1296  short d , off[3];
1297  TreeOctNode::Index( depth , offset , d , off );
1298  int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) };
1299  for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
1300  {
1301  int _offset[] = { x , y , z };
1302  short _d , _off[3];
1303  TreeOctNode::Index( depth , _offset , _d , _off );
1304  int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) };
1305  int symIndex[3];
1306  symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] );
1307  symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] );
1308  symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] );
1309  stencil[x][y][z] = GetLaplacian( symIndex );
1310  }
1311  }
1312 
1313  template< int Degree >
1314  void Octree< Degree >::SetLaplacianStencils( int depth , Stencil< double , 5 > stencils[2][2][2] ) const
1315  {
1316  if( depth<=1 ) return;
1317  for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
1318  {
1319  short d , off[3];
1320  int offset[] = { 4+i , 4+j , 4+k };
1321  TreeOctNode::Index( depth , offset , d , off );
1322  int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) };
1323  for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
1324  {
1325  int _offset[] = { x , y , z };
1326  short _d , _off[3];
1327  TreeOctNode::Index( depth-1 , _offset , _d , _off );
1328  int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) };
1329  int symIndex[3];
1330  symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] );
1331  symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] );
1332  symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] );
1333  stencils[i][j][k].values[x][y][z] = GetLaplacian( symIndex );
1334  }
1335  }
1336  }
1337 
1338  template< int Degree >
1339  void Octree< Degree >::SetDivergenceStencils( int depth , Stencil< Point3D< double > , 5 > stencils[2][2][2] , bool scatter ) const
1340  {
1341  if( depth<=1 ) return;
1342  int index1[3] , index2[3];
1343  for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
1344  {
1345  short d , off[3];
1346  int offset[] = { 4+i , 4+j , 4+k };
1347  TreeOctNode::Index( depth , offset , d , off );
1348  if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
1349  else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
1350  for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
1351  {
1352  int _offset[] = { x , y , z };
1353  TreeOctNode::Index( depth-1 , _offset , d , off );
1354  if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
1355  else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
1356 
1357  int symIndex[] =
1358  {
1359  BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) ,
1360  BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) ,
1361  BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) ,
1362  };
1363  int aSymIndex[] =
1364  {
1365  #if GRADIENT_DOMAIN_SOLUTION
1366  // Take the dot-product of the vector-field with the gradient of the basis function
1367  fData.Index( index1[0] , index2[0] ) ,
1368  fData.Index( index1[1] , index2[1] ) ,
1369  fData.Index( index1[2] , index2[2] )
1370  #else // !GRADIENT_DOMAIN_SOLUTION
1371  // Take the dot-product of the divergence of the vector-field with the basis function
1372  fData.Index( index2[0] , index1[0] ) ,
1373  fData.Index( index2[1] , index1[1] ) ,
1374  fData.Index( index2[2] , index1[2] )
1375  #endif // GRADIENT_DOMAIN_SOLUTION
1376  };
1377  double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
1378 #if GRADIENT_DOMAIN_SOLUTION
1379  stencils[i][j][k].values[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot;
1380  stencils[i][j][k].values[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot;
1381  stencils[i][j][k].values[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot;
1382 #else // !GRADIENT_DOMAIN_SOLUTION
1383  stencils[i][j][k].values[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot;
1384  stencils[i][j][k].values[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot;
1385  stencils[i][j][k].values[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot;
1386 #endif // GRADIENT_DOMAIN_SOLUTION
1387  }
1388  }
1389  }
1390 
1391  template< int Degree >
1392  void Octree< Degree >::SetEvaluationStencils( int depth , Stencil< double , 3 > stencil1[8] , Stencil< double , 3 > stencil2[8][8] ) const
1393  {
1394  if( depth>2 )
1395  {
1396  int idx[3];
1397  int off[] = { 2 , 2 , 2 };
1398  for( int c=0 ; c<8 ; c++ )
1399  {
1400  VertexData::CornerIndex( depth , off , c , fData.depth , idx );
1401  idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount;
1402  for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ )
1403  {
1404  short _d , _off[3];
1405  int _offset[] = { x+1 , y+1 , z+1 };
1406  TreeOctNode::Index( depth , _offset , _d , _off );
1407  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]) ];
1408  }
1409  }
1410  }
1411  if( depth>3 )
1412  for( int _c=0 ; _c<8 ; _c++ )
1413  {
1414  int idx[3];
1415  int _cx , _cy , _cz;
1416  Cube::FactorCornerIndex( _c , _cx , _cy , _cz );
1417  int off[] = { 4+_cx , 4+_cy , 4+_cz };
1418  for( int c=0 ; c<8 ; c++ )
1419  {
1420  VertexData::CornerIndex( depth , off , c , fData.depth , idx );
1421  idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount;
1422  for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ )
1423  {
1424  short _d , _off[3];
1425  int _offset[] = { x+1 , y+1 , z+1 };
1426  TreeOctNode::Index( depth-1 , _offset , _d , _off );
1427  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]) ];
1428  }
1429  }
1430  }
1431  }
1432 
1433  template< int Degree >
1434  void Octree< Degree >::UpdateCoarserSupportBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ )
1435  {
1436  if( node->parent )
1437  {
1438  int x , y , z , c = int( node - node->parent->children );
1439  Cube::FactorCornerIndex( c , x , y , z );
1440  if( x==0 ) endX = 4;
1441  else startX = 1;
1442  if( y==0 ) endY = 4;
1443  else startY = 1;
1444  if( z==0 ) endZ = 4;
1445  else startZ = 1;
1446  }
1447  }
1448 
1449  template< int Degree >
1450  void Octree< Degree >::UpdateConstraintsFromCoarser( const OctNode< TreeNodeData , Real >::NeighborKey5& neighborKey5 , TreeOctNode* node , Real* metSolution , const Stencil< double , 5 >& lapStencil ) const
1451  {
1452  bool isInterior;
1453  {
1454  int d , off[3];
1455  node->depthAndOffset( d , off );
1456  int mn = 4 , mx = (1<<d)-4;
1457  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
1458  }
1459  Real constraint = Real( 0 );
1460  int depth = node->depth();
1461  if( depth<=_minDepth ) return;
1462  int i = node->nodeData.nodeIndex;
1463  // Offset the constraints using the solution from lower resolutions.
1464  int startX = 0 , endX = 5 , startY = 0 , endY = 5 , startZ = 0 , endZ = 5;
1465  UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ );
1466 
1467  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth-1];
1468  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
1469  if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 )
1470  {
1471  const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
1472  Real _solution = metSolution[ _node->nodeData.nodeIndex ];
1473  {
1474  if( isInterior ) node->nodeData.constraint -= Real( lapStencil.values[x][y][z] * _solution );
1475  else node->nodeData.constraint -= GetLaplacian( _node , node ) * _solution;
1476  }
1477  }
1478  if( _constrainValues )
1479  {
1480  int d , idx[3];
1481  node->depthAndOffset( d, idx );
1482  idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] );
1483  idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] );
1484  idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] );
1485  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth];
1486  for( int x=1 ; x<4 ; x++ ) for( int y=1 ; y<4 ; y++ ) for( int z=1 ; z<4 ; z++ )
1487  if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.pointIndex!=-1 )
1488  {
1489  const PointData& pData = _points[ neighbors5.neighbors[x][y][z]->nodeData.pointIndex ];
1490  Real pointValue = pData.value;
1491  Point3D< Real > p = pData.position;
1492  node->nodeData.constraint -=
1493  Real(
1494  fData.baseBSplines[idx[0]][x-1]( p[0] ) *
1495  fData.baseBSplines[idx[1]][y-1]( p[1] ) *
1496  fData.baseBSplines[idx[2]][z-1]( p[2] ) *
1497  pointValue
1498  );
1499  }
1500  }
1501  }
1503  {
1504  int start;
1505  double v[2];
1506  UpSampleData( void ) { start = 0 , v[0] = v[1] = 0.; }
1507  UpSampleData( int s , double v1 , double v2 ) { start = s , v[0] = v1 , v[1] = v2; }
1508  };
1509 
1510  template< int Degree >
1511  void Octree< Degree >::UpSampleCoarserSolution( int depth , const SortedTreeNodes& sNodes , Vector< Real >& Solution ) const
1512  {
1513  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1514  Solution.Resize( range );
1515  if( !depth ) return;
1516  else if( depth==1 ) for( int i=start ; i<end ; i++ ) Solution[i-start] += sNodes.treeNodes[0]->nodeData.solution;
1517  else
1518  {
1519  // For every node at the current depth
1520 #pragma omp parallel for num_threads( threads )
1521  for( int t=0 ; t<threads ; t++ )
1522  {
1523  TreeOctNode::NeighborKey3 neighborKey;
1524  neighborKey.set( depth );
1525  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1526  {
1527  int d , off[3];
1528  UpSampleData usData[3];
1529  sNodes.treeNodes[i]->depthAndOffset( d , off );
1530  for( int d=0 ; d<3 ; d++ )
1531  {
1532  if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
1533  else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
1534  else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
1535  else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
1536  }
1537  neighborKey.getNeighbors( sNodes.treeNodes[i] );
1538  for( int ii=0 ; ii<2 ; ii++ )
1539  {
1540  int _ii = ii + usData[0].start;
1541  double dx = usData[0].v[ii];
1542  for( int jj=0 ; jj<2 ; jj++ )
1543  {
1544  int _jj = jj + usData[1].start;
1545  double dxy = dx * usData[1].v[jj];
1546  for( int kk=0 ; kk<2 ; kk++ )
1547  {
1548  int _kk = kk + usData[2].start;
1549  double dxyz = dxy * usData[2].v[kk];
1550  Solution[i-start] += Real( neighborKey.neighbors[depth-1].neighbors[_ii][_jj][_kk]->nodeData.solution * dxyz );
1551  }
1552  }
1553  }
1554  }
1555  }
1556  }
1557  // Clear the coarser solution
1558  start = sNodes.nodeCount[depth-1] , end = sNodes.nodeCount[depth] , range = end-start;
1559 #pragma omp parallel for num_threads( threads )
1560  for( int i=start ; i<end ; i++ ) sNodes.treeNodes[i]->nodeData.solution = Real( 0. );
1561  }
1562  template< int Degree >
1563  void Octree< Degree >::DownSampleFinerConstraints( int depth , SortedTreeNodes& sNodes ) const
1564  {
1565  if( !depth ) return;
1566 #pragma omp parallel for num_threads( threads )
1567  for( int i=sNodes.nodeCount[depth-1] ; i<sNodes.nodeCount[depth] ; i++ )
1568  sNodes.treeNodes[i]->nodeData.constraint = Real( 0 );
1569 
1570  if( depth==1 )
1571  {
1572  sNodes.treeNodes[0]->nodeData.constraint = Real( 0 );
1573  for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) sNodes.treeNodes[0]->nodeData.constraint += sNodes.treeNodes[i]->nodeData.constraint;
1574  return;
1575  }
1576  std::vector< Vector< double > > constraints( threads );
1577  for( int t=0 ; t<threads ; t++ ) constraints[t].Resize( sNodes.nodeCount[depth] - sNodes.nodeCount[depth-1] ) , constraints[t].SetZero();
1578  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1579  int lStart = sNodes.nodeCount[depth-1] , lEnd = sNodes.nodeCount[depth];
1580  // For every node at the current depth
1581 #pragma omp parallel for num_threads( threads )
1582  for( int t=0 ; t<threads ; t++ )
1583  {
1584  TreeOctNode::NeighborKey3 neighborKey;
1585  neighborKey.set( depth );
1586  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1587  {
1588  int d , off[3];
1589  UpSampleData usData[3];
1590  sNodes.treeNodes[i]->depthAndOffset( d , off );
1591  for( int d=0 ; d<3 ; d++ )
1592  {
1593  if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
1594  else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
1595  else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
1596  else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
1597  }
1598  neighborKey.getNeighbors( sNodes.treeNodes[i] );
1599  TreeOctNode::Neighbors3& neighbors = neighborKey.neighbors[depth-1];
1600  for( int ii=0 ; ii<2 ; ii++ )
1601  {
1602  int _ii = ii + usData[0].start;
1603  double dx = usData[0].v[ii];
1604  for( int jj=0 ; jj<2 ; jj++ )
1605  {
1606  int _jj = jj + usData[1].start;
1607  double dxy = dx * usData[1].v[jj];
1608  for( int kk=0 ; kk<2 ; kk++ )
1609  {
1610  int _kk = kk + usData[2].start;
1611  double dxyz = dxy * usData[2].v[kk];
1612  constraints[t][neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex-lStart] += sNodes.treeNodes[i]->nodeData.constraint * dxyz;
1613  }
1614  }
1615  }
1616  }
1617  }
1618 #pragma omp parallel for num_threads( threads )
1619  for( int i=lStart ; i<lEnd ; i++ )
1620  {
1621  Real cSum = Real(0.);
1622  for( int t=0 ; t<threads ; t++ ) cSum += constraints[t][i-lStart];
1623  sNodes.treeNodes[i]->nodeData.constraint += cSum;
1624  }
1625  }
1626  template< int Degree >
1627  template< class C >
1628  void Octree< Degree >::DownSample( int depth , const SortedTreeNodes& sNodes , C* constraints ) const
1629  {
1630  if( depth==0 ) return;
1631  if( depth==1 )
1632  {
1633  for( int i=sNodes.nodeCount[1] ; i<sNodes.nodeCount[2] ; i++ ) constraints[0] += constraints[i];
1634  return;
1635  }
1636  std::vector< Vector< C > > _constraints( threads );
1637  for( int t=0 ; t<threads ; t++ ) _constraints[t].Resize( sNodes.nodeCount[depth] - sNodes.nodeCount[depth-1] );
1638  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start , lStart = sNodes.nodeCount[depth-1] , lEnd = sNodes.nodeCount[depth];
1639  // For every node at the current depth
1640 #pragma omp parallel for num_threads( threads )
1641  for( int t=0 ; t<threads ; t++ )
1642  {
1643  TreeOctNode::NeighborKey3 neighborKey;
1644  neighborKey.set( depth );
1645  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1646  {
1647  int d , off[3];
1648  UpSampleData usData[3];
1649  sNodes.treeNodes[i]->depthAndOffset( d , off );
1650  for( int d=0 ; d<3 ; d++ )
1651  {
1652  if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
1653  else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
1654  else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
1655  else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
1656  }
1657  TreeOctNode::Neighbors3& neighbors = neighborKey.getNeighbors( sNodes.treeNodes[i]->parent );
1658  C c = constraints[i];
1659  for( int ii=0 ; ii<2 ; ii++ )
1660  {
1661  int _ii = ii + usData[0].start;
1662  C cx = C( c*usData[0].v[ii] );
1663  for( int jj=0 ; jj<2 ; jj++ )
1664  {
1665  int _jj = jj + usData[1].start;
1666  C cxy = C( cx*usData[1].v[jj] );
1667  for( int kk=0 ; kk<2 ; kk++ )
1668  {
1669  int _kk = kk + usData[2].start;
1670  if( neighbors.neighbors[_ii][_jj][_kk] )
1671  _constraints[t][neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex-lStart] += C( cxy*usData[2].v[kk] );
1672  }
1673  }
1674  }
1675  }
1676  }
1677 #pragma omp parallel for num_threads( threads )
1678  for( int i=lStart ; i<lEnd ; i++ )
1679  {
1680  C cSum = C(0);
1681  for( int t=0 ; t<threads ; t++ ) cSum += _constraints[t][i-lStart];
1682  constraints[i] += cSum;
1683  }
1684  }
1685 
1686  template< int Degree >
1687  template< class C >
1688  void Octree< Degree >::UpSample( int depth , const SortedTreeNodes& sNodes , C* coefficients ) const
1689  {
1690  if ( depth==0 ) return;
1691  else if( depth==1 )
1692  {
1693  for( int i=sNodes.nodeCount[1] ; i<sNodes.nodeCount[2] ; i++ ) coefficients[i] += coefficients[0];
1694  return;
1695  }
1696 
1697  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1698  // For every node at the current depth
1699 #pragma omp parallel for num_threads( threads )
1700  for( int t=0 ; t<threads ; t++ )
1701  {
1702  TreeOctNode::NeighborKey3 neighborKey;
1703  neighborKey.set( depth-1 );
1704  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1705  {
1706  TreeOctNode* node = sNodes.treeNodes[i];
1707  int d , off[3];
1708  UpSampleData usData[3];
1709  node->depthAndOffset( d , off );
1710  for( int d=0 ; d<3 ; d++ )
1711  {
1712  if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
1713  else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
1714  else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
1715  else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
1716  }
1717  TreeOctNode::Neighbors3& neighbors = neighborKey.getNeighbors( node->parent );
1718  for( int ii=0 ; ii<2 ; ii++ )
1719  {
1720  int _ii = ii + usData[0].start;
1721  double dx = usData[0].v[ii];
1722  for( int jj=0 ; jj<2 ; jj++ )
1723  {
1724  int _jj = jj + usData[1].start;
1725  double dxy = dx * usData[1].v[jj];
1726  for( int kk=0 ; kk<2 ; kk++ )
1727  {
1728  int _kk = kk + usData[2].start;
1729  if( neighbors.neighbors[_ii][_jj][_kk] )
1730  {
1731  double dxyz = dxy * usData[2].v[kk];
1732  int _i = neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex;
1733  coefficients[i] += coefficients[_i] * Real( dxyz );
1734  }
1735  }
1736  }
1737  }
1738  }
1739  }
1740  }
1741 
1742  template< int Degree >
1743  void Octree< Degree >::SetCoarserPointValues( int depth , const SortedTreeNodes& sNodes , Real* metSolution )
1744  {
1745  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1746  // For every node at the current depth
1747 #pragma omp parallel for num_threads( threads )
1748  for( int t=0 ; t<threads ; t++ )
1749  {
1750  TreeOctNode::NeighborKey3 neighborKey;
1751  neighborKey.set( depth );
1752  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
1753  {
1754  int pIdx = sNodes.treeNodes[i]->nodeData.pointIndex;
1755  if( pIdx!=-1 )
1756  {
1757  neighborKey.getNeighbors( sNodes.treeNodes[i] );
1758  _points[ pIdx ].value = WeightedCoarserFunctionValue( neighborKey , sNodes.treeNodes[i] , metSolution );
1759  }
1760  }
1761  }
1762  }
1763 
1764  template< int Degree >
1765  Real Octree< Degree >::WeightedCoarserFunctionValue( const OctNode< TreeNodeData , Real >::NeighborKey3& neighborKey , const TreeOctNode* pointNode , Real* metSolution ) const
1766  {
1767  int depth = pointNode->depth();
1768  if( !depth || pointNode->nodeData.pointIndex==-1 ) return Real(0.);
1769  double pointValue = 0;
1770 
1771  Real weight = _points[ pointNode->nodeData.pointIndex ].weight;
1772  Point3D< Real > p = _points[ pointNode->nodeData.pointIndex ].position;
1773 
1774  // Iterate over all basis functions that overlap the point at the coarser resolutions
1775  {
1776  int d , _idx[3];
1777  const TreeOctNode::Neighbors3& neighbors = neighborKey.neighbors[depth-1];
1778  neighbors.neighbors[1][1][1]->depthAndOffset( d , _idx );
1779  _idx[0] = BinaryNode< double >::CenterIndex( d , _idx[0]-1 );
1780  _idx[1] = BinaryNode< double >::CenterIndex( d , _idx[1]-1 );
1781  _idx[2] = BinaryNode< double >::CenterIndex( d , _idx[2]-1 );
1782  for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) for( int l=0 ; l<3 ; l++ )
1783  if( neighbors.neighbors[j][k][l] && neighbors.neighbors[j][k][l]->nodeData.nodeIndex>=0 )
1784  {
1785  // Accumulate the contribution from these basis nodes
1786  const TreeOctNode* basisNode = neighbors.neighbors[j][k][l];
1787  int idx[] = { _idx[0]+j , _idx[1]+k , _idx[2]+l };
1788  pointValue +=
1789  fData.baseBSplines[ idx[0] ][2-j]( p[0] ) *
1790  fData.baseBSplines[ idx[1] ][2-k]( p[1] ) *
1791  fData.baseBSplines[ idx[2] ][2-l]( p[2] ) *
1792  metSolution[basisNode->nodeData.nodeIndex];
1793  }
1794  }
1795  return Real( pointValue * weight );
1796  }
1797 
1798  template<int Degree>
1799  int Octree<Degree>::GetFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix , int depth , const SortedTreeNodes& sNodes , Real* metSolution )
1800  {
1801  int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
1802  double stencil[5][5][5];
1803  SetLaplacianStencil( depth , stencil );
1804  Stencil< double , 5 > stencils[2][2][2];
1805  SetLaplacianStencils( depth , stencils );
1806  matrix.Resize( range );
1807 #pragma omp parallel for num_threads( threads )
1808  for( int t=0 ; t<threads ; t++ )
1809  {
1810  TreeOctNode::NeighborKey5 neighborKey5;
1811  neighborKey5.set( depth );
1812  for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ )
1813  {
1814  TreeOctNode* node = sNodes.treeNodes[i+start];
1815  neighborKey5.getNeighbors( node );
1816 
1817  // Get the matrix row size
1818  int count = GetMatrixRowSize( neighborKey5.neighbors[depth] );
1819 
1820  // Allocate memory for the row
1821 #pragma omp critical (matrix_set_row_size)
1822  {
1823  matrix.SetRowSize( i , count );
1824  }
1825 
1826  // Set the row entries
1827  matrix.rowSizes[i] = SetMatrixRow( neighborKey5.neighbors[depth] , matrix[i] , start , stencil );
1828 
1829  // Offset the constraints using the solution from lower resolutions.
1830  int x , y , z , c;
1831  if( node->parent )
1832  {
1833  c = int( node - node->parent->children );
1834  Cube::FactorCornerIndex( c , x , y , z );
1835  }
1836  else x = y = z = 0;
1837  UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] );
1838  }
1839  }
1840  return 1;
1841  }
1842 
1843  template<int Degree>
1844  int Octree<Degree>::GetRestrictedFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix,int depth,const int* entries,int entryCount,
1845  const TreeOctNode* rNode , Real radius ,
1846  const SortedTreeNodes& sNodes , Real* metSolution )
1847  {
1848  for( int i=0 ; i<entryCount ; i++ ) sNodes.treeNodes[ entries[i] ]->nodeData.nodeIndex = i;
1849  double stencil[5][5][5];
1850  int rDepth , rOff[3];
1851  rNode->depthAndOffset( rDepth , rOff );
1852  matrix.Resize( entryCount );
1853  SetLaplacianStencil( depth , stencil );
1854  Stencil< double , 5 > stencils[2][2][2];
1855  SetLaplacianStencils( depth , stencils );
1856 #pragma omp parallel for num_threads( threads )
1857  for( int t=0 ; t<threads ; t++ )
1858  {
1859  TreeOctNode::NeighborKey5 neighborKey5;
1860  neighborKey5.set( depth );
1861  for( int i=(entryCount*t)/threads ; i<(entryCount*(t+1))/threads ; i++ )
1862  {
1863  TreeOctNode* node = sNodes.treeNodes[ entries[i] ];
1864  int d , off[3];
1865  node->depthAndOffset( d , off );
1866  off[0] >>= (depth-rDepth) , off[1] >>= (depth-rDepth) , off[2] >>= (depth-rDepth);
1867  bool isInterior = ( off[0]==rOff[0] && off[1]==rOff[1] && off[2]==rOff[2] );
1868 
1869  neighborKey5.getNeighbors( node );
1870 
1871  int xStart=0 , xEnd=5 , yStart=0 , yEnd=5 , zStart=0 , zEnd=5;
1872  if( !isInterior ) SetMatrixRowBounds( neighborKey5.neighbors[depth].neighbors[2][2][2] , rDepth , rOff , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1873 
1874  // Get the matrix row size
1875  int count = GetMatrixRowSize( neighborKey5.neighbors[depth] , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1876 
1877  // Allocate memory for the row
1878 #pragma omp critical (matrix_set_row_size)
1879  {
1880  matrix.SetRowSize( i , count );
1881  }
1882 
1883  // Set the matrix row entries
1884  matrix.rowSizes[i] = SetMatrixRow( neighborKey5.neighbors[depth] , matrix[i] , 0 , stencil , xStart , xEnd , yStart , yEnd , zStart , zEnd );
1885  // Adjust the system constraints
1886  int x , y , z , c;
1887  if( node->parent )
1888  {
1889  c = int( node - node->parent->children );
1890  Cube::FactorCornerIndex( c , x , y , z );
1891  }
1892  else x = y = z = 0;
1893  UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] );
1894  }
1895  }
1896  for( int i=0 ; i<entryCount ; i++ ) sNodes.treeNodes[entries[i]]->nodeData.nodeIndex = entries[i];
1897  return 1;
1898  }
1899 
1900  template<int Degree>
1901  int Octree<Degree>::LaplacianMatrixIteration( int subdivideDepth , bool showResidual , int minIters , double accuracy )
1902  {
1903  int i,iter=0;
1904  double t = 0;
1905  fData.setDotTables( fData.DD_DOT_FLAG | fData.DV_DOT_FLAG );
1906 
1907  SparseMatrix< float >::SetAllocator( MEMORY_ALLOCATOR_BLOCK_SIZE );
1908  _sNodes.treeNodes[0]->nodeData.solution = 0;
1909 
1910  std::vector< Real > metSolution( _sNodes.nodeCount[ _sNodes.maxDepth ] , 0 );
1911 
1912  for( i=1 ; i<_sNodes.maxDepth ; i++ )
1913  {
1914  if( subdivideDepth>0 ) iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , subdivideDepth , showResidual , minIters , accuracy );
1915  else iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , showResidual , minIters , accuracy );
1916  }
1918  fData.clearDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG | fData.DD_DOT_FLAG );
1919 
1920  return iter;
1921  }
1922 
1923  template<int Degree>
1924  int Octree<Degree>::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , bool showResidual , int minIters , double accuracy )
1925  {
1926  int iter = 0;
1927  Vector< Real > X , B;
1929  double systemTime=0. , solveTime=0. , updateTime=0. , evaluateTime = 0.;
1930 
1931  X.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] );
1932  if( depth<=_minDepth ) UpSampleCoarserSolution( depth , sNodes , X );
1933  else
1934  {
1935  // Up-sample the cumulative solution into the previous depth
1936  UpSample( depth-1 , sNodes , metSolution );
1937  // Add in the solution from that depth
1938  if( depth )
1939 #pragma omp parallel for num_threads( threads )
1940  for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution;
1941  }
1942  if( _constrainValues )
1943  {
1944  SetCoarserPointValues( depth , sNodes , metSolution );
1945  }
1946 
1948  {
1949  int maxECount = ( (2*Degree+1)*(2*Degree+1)*(2*Degree+1) + 1 ) / 2;
1950  maxECount = ( ( maxECount + 15 ) / 16 ) * 16;
1951  M.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] );
1952  for( int i=0 ; i<M.rows ; i++ ) M.SetRowSize( i , maxECount );
1953  }
1954 
1955  {
1956  // Get the system matrix
1958  GetFixedDepthLaplacian( M , depth , sNodes , metSolution );
1959  // Set the constraint vector
1960  B.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] );
1961  for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) B[i-sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.constraint;
1962  }
1963 
1964  // Solve the linear system
1965  iter += SparseSymmetricMatrix< Real >::Solve( M , B , std::max< int >( int( pow( M.rows , ITERATION_POWER ) ) , minIters ) , X , Real(accuracy) , 0 , threads , (depth<=_minDepth) && !_constrainValues );
1966 
1967  if( showResidual )
1968  {
1969  double mNorm = 0;
1970  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;
1971  double bNorm = B.Norm( 2 ) , rNorm = ( B - M * X ).Norm( 2 );
1972  printf( "\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter );
1973  }
1974 
1975  // Copy the solution back into the tree (over-writing the constraints)
1976  for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) sNodes.treeNodes[i]->nodeData.solution = Real( X[i-sNodes.nodeCount[depth]] );
1977 
1978  return iter;
1979  }
1980 
1981  template<int Degree>
1982  int Octree<Degree>::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , int startingDepth , bool showResidual , int minIters , double accuracy )
1983  {
1984  if( startingDepth>=depth ) return SolveFixedDepthMatrix( depth , sNodes , metSolution , showResidual , minIters , accuracy );
1985 
1986  int i , j , d , tIter=0;
1988  Vector< Real > B , B_ , X_;
1989  AdjacencySetFunction asf;
1990  AdjacencyCountFunction acf;
1991  double systemTime = 0 , solveTime = 0 , memUsage = 0 , evaluateTime = 0 , gTime = 0, sTime = 0;
1992  Real myRadius , myRadius2;
1993 
1994  if( depth>_minDepth )
1995  {
1996  // Up-sample the cumulative solution into the previous depth
1997  UpSample( depth-1 , sNodes , metSolution );
1998  // Add in the solution from that depth
1999  if( depth )
2000 #pragma omp parallel for num_threads( threads )
2001  for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution;
2002  }
2003 
2004  if( _constrainValues )
2005  {
2006  SetCoarserPointValues( depth , sNodes , metSolution );
2007  }
2008  B.Resize( sNodes.nodeCount[depth+1] - sNodes.nodeCount[depth] );
2009 
2010  // Back-up the constraints
2011  for( i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ )
2012  {
2013  B[ i-sNodes.nodeCount[depth] ] = sNodes.treeNodes[i]->nodeData.constraint;
2014  sNodes.treeNodes[i]->nodeData.constraint = 0;
2015  }
2016 
2017  myRadius = 2*radius-Real(0.5);
2018  myRadius = int(myRadius-ROUND_EPS)+ROUND_EPS;
2019  myRadius2 = Real(radius+ROUND_EPS-0.5);
2020  d = depth-startingDepth;
2021  std::vector< int > subDimension( sNodes.nodeCount[d+1]-sNodes.nodeCount[d] );
2022  int maxDimension = 0;
2023  for( i=sNodes.nodeCount[d] ; i<sNodes.nodeCount[d+1] ; i++ )
2024  {
2025  // Count the number of nodes at depth "depth" that lie under sNodes.treeNodes[i]
2026  acf.adjacencyCount = 0;
2027  for( TreeOctNode* temp=sNodes.treeNodes[i]->nextNode() ; temp ; )
2028  {
2029  if( temp->depth()==depth ) acf.adjacencyCount++ , temp = sNodes.treeNodes[i]->nextBranch( temp );
2030  else temp = sNodes.treeNodes[i]->nextNode ( temp );
2031  }
2032  for( j=sNodes.nodeCount[d] ; j<sNodes.nodeCount[d+1] ; j++ )
2033  {
2034  if( i==j ) continue;
2035  TreeOctNode::ProcessFixedDepthNodeAdjacentNodes( fData.depth , sNodes.treeNodes[i] , 1 , sNodes.treeNodes[j] , 2*width-1 , depth , &acf );
2036  }
2037  subDimension[i-sNodes.nodeCount[d]] = acf.adjacencyCount;
2038  maxDimension = std::max< int >( maxDimension , subDimension[i-sNodes.nodeCount[d]] );
2039  }
2040  asf.adjacencies = new int[maxDimension];
2041  MapReduceVector< Real > mrVector;
2042  mrVector.resize( threads , maxDimension );
2043  // Iterate through the coarse-level nodes
2044  for( i=sNodes.nodeCount[d] ; i<sNodes.nodeCount[d+1] ; i++ )
2045  {
2046  int iter = 0;
2047  // Count the number of nodes at depth "depth" that lie under sNodes.treeNodes[i]
2048  acf.adjacencyCount = subDimension[i-sNodes.nodeCount[d]];
2049  if( !acf.adjacencyCount ) continue;
2050 
2051  // Set the indices for the nodes under, or near, sNodes.treeNodes[i].
2052  asf.adjacencyCount = 0;
2053  for( TreeOctNode* temp=sNodes.treeNodes[i]->nextNode() ; temp ; )
2054  {
2055  if( temp->depth()==depth ) asf.adjacencies[ asf.adjacencyCount++ ] = temp->nodeData.nodeIndex , temp = sNodes.treeNodes[i]->nextBranch( temp );
2056  else temp = sNodes.treeNodes[i]->nextNode ( temp );
2057  }
2058  for( j=sNodes.nodeCount[d] ; j<sNodes.nodeCount[d+1] ; j++ )
2059  {
2060  if( i==j ) continue;
2061  TreeOctNode::ProcessFixedDepthNodeAdjacentNodes( fData.depth , sNodes.treeNodes[i] , 1 , sNodes.treeNodes[j] , 2*width-1 , depth , &asf );
2062  }
2063 
2064  // Get the associated constraint vector
2065  B_.Resize( asf.adjacencyCount );
2066  for( j=0 ; j<asf.adjacencyCount ; j++ ) B_[j] = B[ asf.adjacencies[j]-sNodes.nodeCount[depth] ];
2067 
2068  X_.Resize( asf.adjacencyCount );
2069 #pragma omp parallel for num_threads( threads ) schedule( static )
2070  for( j=0 ; j<asf.adjacencyCount ; j++ )
2071  {
2072  X_[j] = sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution;
2073  }
2074  // Get the associated matrix
2076  GetRestrictedFixedDepthLaplacian( _M , depth , asf.adjacencies , asf.adjacencyCount , sNodes.treeNodes[i] , myRadius , sNodes , metSolution );
2077 #pragma omp parallel for num_threads( threads ) schedule( static )
2078  for( j=0 ; j<asf.adjacencyCount ; j++ )
2079  {
2080  B_[j] += sNodes.treeNodes[asf.adjacencies[j]]->nodeData.constraint;
2081  sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.constraint = 0;
2082  }
2083 
2084  // Solve the matrix
2085  // Since we don't have the full matrix, the system shouldn't be singular, so we shouldn't have to correct it
2086  iter += SparseSymmetricMatrix< Real >::Solve( _M , B_ , std::max< int >( int( pow( _M.rows , ITERATION_POWER ) ) , minIters ) , X_ , mrVector , Real(accuracy) , 0 );
2087 
2088  if( showResidual )
2089  {
2090  double mNorm = 0;
2091  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;
2092  double bNorm = B_.Norm( 2 ) , rNorm = ( B_ - _M * X_ ).Norm( 2 );
2093  printf( "\t\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , _M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter );
2094  }
2095 
2096  // Update the solution for all nodes in the sub-tree
2097  for( j=0 ; j<asf.adjacencyCount ; j++ )
2098  {
2099  TreeOctNode* temp=sNodes.treeNodes[ asf.adjacencies[j] ];
2100  while( temp->depth()>sNodes.treeNodes[i]->depth() ) temp=temp->parent;
2101  if( temp->nodeData.nodeIndex>=sNodes.treeNodes[i]->nodeData.nodeIndex ) sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution = Real( X_[j] );
2102  }
2103  systemTime += gTime;
2104  solveTime += sTime;
2105  memUsage = std::max< double >( MemoryUsage() , memUsage );
2106  tIter += iter;
2107  }
2108  delete[] asf.adjacencies;
2109  return tIter;
2110  }
2111 
2112  template<int Degree>
2113  int Octree<Degree>::HasNormals(TreeOctNode* node,Real epsilon)
2114  {
2115  int hasNormals=0;
2116  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;
2117  if( node->children ) for(int i=0;i<Cube::CORNERS && !hasNormals;i++) hasNormals |= HasNormals(&node->children[i],epsilon);
2118 
2119  return hasNormals;
2120  }
2121 
2122  template<int Degree>
2124  {
2125  int maxDepth = tree.maxDepth();
2126  for( TreeOctNode* temp=tree.nextNode() ; temp ; temp=tree.nextNode(temp) )
2127  if( temp->children && temp->d>=_minDepth )
2128  {
2129  int hasNormals=0;
2130  for( int i=0 ; i<Cube::CORNERS && !hasNormals ; i++ ) hasNormals = HasNormals( &temp->children[i] , EPSILON/(1<<maxDepth) );
2131  if( !hasNormals ) temp->children=NULL;
2132  }
2133  MemoryUsage();
2134  }
2135 
2136  template<int Degree>
2138  {
2139  // To set the Laplacian constraints, we iterate over the
2140  // splatted normals and compute the dot-product of the
2141  // divergence of the normal field with all the basis functions.
2142  // Within the same depth: set directly as a gather
2143  // Coarser depths
2144  fData.setDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG );
2145 
2146  int maxDepth = _sNodes.maxDepth-1;
2147  Point3D< Real > zeroPoint;
2148  zeroPoint[0] = zeroPoint[1] = zeroPoint[2] = 0;
2149  std::vector< Real > constraints( _sNodes.nodeCount[maxDepth] , Real(0) );
2150  std::vector< Point3D< Real > > coefficients( _sNodes.nodeCount[maxDepth] , zeroPoint );
2151 
2152  // Clear the constraints
2153 #pragma omp parallel for num_threads( threads )
2154  for( int i=0 ; i<_sNodes.nodeCount[maxDepth+1] ; i++ ) _sNodes.treeNodes[i]->nodeData.constraint = Real( 0. );
2155 
2156  // For the scattering part of the operation, we parallelize by duplicating the constraints and then summing at the end.
2157  std::vector< std::vector< Real > > _constraints( threads );
2158  for( int t=0 ; t<threads ; t++ ) _constraints[t].resize( _sNodes.nodeCount[maxDepth] , 0 );
2159 
2160  for( int d=maxDepth ; d>=0 ; d-- )
2161  {
2162  Point3D< double > stencil[5][5][5];
2163  SetDivergenceStencil( d , &stencil[0][0][0] , false );
2164  Stencil< Point3D< double > , 5 > stencils[2][2][2];
2165  SetDivergenceStencils( d , stencils , true );
2166 #pragma omp parallel for num_threads( threads )
2167  for( int t=0 ; t<threads ; t++ )
2168  {
2169  TreeOctNode::NeighborKey5 neighborKey5;
2170  neighborKey5.set( fData.depth );
2171  int start = _sNodes.nodeCount[d] , end = _sNodes.nodeCount[d+1] , range = end-start;
2172  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
2173  {
2174  TreeOctNode* node = _sNodes.treeNodes[i];
2175  int startX=0 , endX=5 , startY=0 , endY=5 , startZ=0 , endZ=5;
2176  int depth = node->depth();
2177  neighborKey5.getNeighbors( node );
2178 
2179  bool isInterior , isInterior2;
2180  {
2181  int d , off[3];
2182  node->depthAndOffset( d , off );
2183  int mn = 2 , mx = (1<<d)-2;
2184  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
2185  mn += 2 , mx -= 2;
2186  isInterior2 = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
2187  }
2188  int cx , cy , cz;
2189  if( d )
2190  {
2191  int c = int( node - node->parent->children );
2192  Cube::FactorCornerIndex( c , cx , cy , cz );
2193  }
2194  else cx = cy = cz = 0;
2195  Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz];
2196 
2197  // Set constraints from current depth
2198  {
2199  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth];
2200 
2201  if( isInterior )
2202  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2203  {
2204  const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
2205  if( _node && _node->nodeData.normalIndex>=0 )
2206  {
2207  const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex];
2208  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] );
2209  }
2210  }
2211  else
2212  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2213  {
2214  const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
2215  if( _node && _node->nodeData.normalIndex>=0 )
2216  {
2217  const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex];
2218  node->nodeData.constraint += GetDivergence( _node , node , _normal );
2219  }
2220  }
2221  UpdateCoarserSupportBounds( neighbors5.neighbors[2][2][2] , startX , endX , startY , endY , startZ , endZ );
2222  }
2223  if( node->nodeData.nodeIndex<0 || node->nodeData.normalIndex<0 ) continue;
2224  const Point3D< Real >& normal = (*normals)[node->nodeData.normalIndex];
2225  if( normal[0]==0 && normal[1]==0 && normal[2]==0 ) continue;
2226  if( depth<maxDepth ) coefficients[i] += normal;
2227 
2228  if( depth )
2229  {
2230  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth-1];
2231 
2232  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2233  if( neighbors5.neighbors[x][y][z] )
2234  {
2235  TreeOctNode* _node = neighbors5.neighbors[x][y][z];
2236  if( isInterior2 )
2237  {
2238  Point3D< double >& div = _stencil.values[x][y][z];
2239  _constraints[t][ _node->nodeData.nodeIndex ] += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] );
2240  }
2241  else _constraints[t][ _node->nodeData.nodeIndex ] += GetDivergence( node , _node , normal );
2242  }
2243  }
2244  }
2245  }
2246  }
2247 #pragma omp parallel for num_threads( threads ) schedule( static )
2248  for( int i=0 ; i<_sNodes.nodeCount[maxDepth] ; i++ )
2249  {
2250  Real cSum = Real(0.);
2251  for( int t=0 ; t<threads ; t++ ) cSum += _constraints[t][i];
2252  constraints[i] = cSum;
2253  }
2254  // Fine-to-coarse down-sampling of constraints
2255  for( int d=maxDepth-1 ; d>=0 ; d-- ) DownSample( d , _sNodes , &constraints[0] );
2256 
2257  // Coarse-to-fine up-sampling of coefficients
2258  for( int d=0 ; d<maxDepth ; d++ ) UpSample( d , _sNodes , &coefficients[0] );
2259 
2260  // Add the accumulated constraints from all finer depths
2261 #pragma omp parallel for num_threads( threads )
2262  for( int i=0 ; i<_sNodes.nodeCount[maxDepth] ; i++ ) _sNodes.treeNodes[i]->nodeData.constraint += constraints[i];
2263 
2264  // Compute the contribution from all coarser depths
2265  for( int d=0 ; d<=maxDepth ; d++ )
2266  {
2267  int start = _sNodes.nodeCount[d] , end = _sNodes.nodeCount[d+1] , range = end - start;
2268  Stencil< Point3D< double > , 5 > stencils[2][2][2];
2269  SetDivergenceStencils( d , stencils , false );
2270 #pragma omp parallel for num_threads( threads )
2271  for( int t=0 ; t<threads ; t++ )
2272  {
2273  TreeOctNode::NeighborKey5 neighborKey5;
2274  neighborKey5.set( maxDepth );
2275  for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
2276  {
2277  TreeOctNode* node = _sNodes.treeNodes[i];
2278  int depth = node->depth();
2279  if( !depth ) continue;
2280  int startX=0 , endX=5 , startY=0 , endY=5 , startZ=0 , endZ=5;
2281  UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ );
2282  const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.getNeighbors( node->parent );
2283 
2284  bool isInterior;
2285  {
2286  int d , off[3];
2287  node->depthAndOffset( d , off );
2288  int mn = 4 , mx = (1<<d)-4;
2289  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
2290  }
2291  int cx , cy , cz;
2292  if( d )
2293  {
2294  int c = int( node - node->parent->children );
2295  Cube::FactorCornerIndex( c , cx , cy , cz );
2296  }
2297  else cx = cy = cz = 0;
2298  Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz];
2299 
2300  Real constraint = Real(0);
2301  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2302  if( neighbors5.neighbors[x][y][z] )
2303  {
2304  TreeOctNode* _node = neighbors5.neighbors[x][y][z];
2305  int _i = _node->nodeData.nodeIndex;
2306  if( isInterior )
2307  {
2308  Point3D< double >& div = _stencil.values[x][y][z];
2309  Point3D< Real >& normal = coefficients[_i];
2310  constraint += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] );
2311  }
2312  else constraint += GetDivergence( _node , node , coefficients[_i] );
2313  }
2314  node->nodeData.constraint += constraint;
2315  }
2316  }
2317  }
2318 
2319  fData.clearDotTables( fData.DV_DOT_FLAG );
2320 
2321  // Set the point weights for evaluating the iso-value
2322 #pragma omp parallel for num_threads( threads )
2323  for( int t=0 ; t<threads ; t++ )
2324  for( int i=(_sNodes.nodeCount[maxDepth+1]*t)/threads ; i<(_sNodes.nodeCount[maxDepth+1]*(t+1))/threads ; i++ )
2325  {
2326  TreeOctNode* temp = _sNodes.treeNodes[i];
2327  if( temp->nodeData.nodeIndex<0 || temp->nodeData.normalIndex<0 ) temp->nodeData.centerWeightContribution = 0;
2328  else temp->nodeData.centerWeightContribution = Real( Length((*normals)[temp->nodeData.normalIndex]) );
2329  }
2330  MemoryUsage();
2331  delete normals;
2332  normals = NULL;
2333  }
2334 
2335  template<int Degree>
2336  void Octree<Degree>::AdjacencyCountFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencyCount++;}
2337 
2338  template<int Degree>
2339  void Octree<Degree>::AdjacencySetFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencies[adjacencyCount++]=node1->nodeData.nodeIndex;}
2340 
2341  template<int Degree>
2343  {
2344  if( !node1->children && node1->depth()<depth ) node1->initChildren();
2345  }
2346 
2347  template< int Degree >
2348  void Octree< Degree >::FaceEdgesFunction::Function( const TreeOctNode* node1 , const TreeOctNode* node2 )
2349  {
2350  if( !node1->children && MarchingCubes::HasRoots( node1->nodeData.mcIndex ) )
2351  {
2352  RootInfo ri1 , ri2;
2353  int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES];
2354  int count=MarchingCubes::AddTriangleIndices( node1->nodeData.mcIndex , isoTri );
2355 
2356  for( int j=0 ; j<count ; j++ )
2357  for( int k=0 ; k<3 ; k++ )
2358  if( fIndex==Cube::FaceAdjacentToEdges( isoTri[j*3+k] , isoTri[j*3+((k+1)%3)] ) )
2359  if( GetRootIndex( node1 , isoTri[j*3+k] , maxDepth , ri1 ) && GetRootIndex( node1 , isoTri[j*3+((k+1)%3)] , maxDepth , ri2 ) )
2360  {
2361  long long key1=ri1.key , key2=ri2.key;
2362  edges->push_back( std::pair< RootInfo , RootInfo >( ri2 , ri1 ) );
2363  if( vertexCount->count( key1 )==0 )
2364  {
2365  (*vertexCount)[key1].first = ri1;
2366  (*vertexCount)[key1].second=0;
2367  }
2368  if( vertexCount->count( key2 )==0 )
2369  {
2370  (*vertexCount)[key2].first = ri2;
2371  (*vertexCount)[key2].second=0;
2372  }
2373  (*vertexCount)[key1].second--;
2374  (*vertexCount)[key2].second++;
2375  }
2376  else fprintf( stderr , "Bad Edge 1: %d %d\n" , ri1.key , ri2.key );
2377  }
2378  }
2379 
2380  template< int Degree >
2381  void Octree< Degree >::RefineBoundary( int subdivideDepth )
2382  {
2383  // This implementation is somewhat tricky.
2384  // We would like to ensure that leaf-nodes across a subdivision boundary have the same depth.
2385  // We do this by calling the setNeighbors function.
2386  // The key is to implement this in a single pass through the leaves, ensuring that refinements don't propogate.
2387  // To this end, we do the minimal refinement that ensures that a cross boundary neighbor, and any of its cross-boundary
2388  // neighbors are all refined simultaneously.
2389  // For this reason, the implementation can only support nodes deeper than sDepth.
2390  bool flags[3][3][3];
2391  int maxDepth = tree.maxDepth();
2392 
2393  int sDepth;
2394  if( subdivideDepth<=0 ) sDepth = 0;
2395  else sDepth = maxDepth-subdivideDepth;
2396  if( sDepth<=0 ) return;
2397 
2398  // Ensure that face adjacent neighbors across the subdivision boundary exist to allow for
2399  // a consistent definition of the iso-surface
2400  TreeOctNode::NeighborKey3 nKey;
2401  nKey.set( maxDepth );
2402  for( TreeOctNode* leaf=tree.nextLeaf() ; leaf ; leaf=tree.nextLeaf( leaf ) )
2403  if( leaf->depth()>sDepth )
2404  {
2405  int d , off[3] , _off[3];
2406  leaf->depthAndOffset( d , off );
2407  int res = (1<<d)-1 , _res = ( 1<<(d-sDepth) )-1;
2408  _off[0] = off[0]&_res , _off[1] = off[1]&_res , _off[2] = off[2]&_res;
2409  bool boundary[3][2] =
2410  {
2411  { (off[0]!=0 && _off[0]==0) , (off[0]!=res && _off[0]==_res) } ,
2412  { (off[1]!=0 && _off[1]==0) , (off[1]!=res && _off[1]==_res) } ,
2413  { (off[2]!=0 && _off[2]==0) , (off[2]!=res && _off[2]==_res) }
2414  };
2415 
2416  if( boundary[0][0] || boundary[0][1] || boundary[1][0] || boundary[1][1] || boundary[2][0] || boundary[2][1] )
2417  {
2418  TreeOctNode::Neighbors3& neighbors = nKey.getNeighbors( leaf );
2419  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;
2420  int x=0 , y=0 , z=0;
2421  if ( boundary[0][0] && !neighbors.neighbors[0][1][1] ) x = -1;
2422  else if( boundary[0][1] && !neighbors.neighbors[2][1][1] ) x = 1;
2423  if ( boundary[1][0] && !neighbors.neighbors[1][0][1] ) y = -1;
2424  else if( boundary[1][1] && !neighbors.neighbors[1][2][1] ) y = 1;
2425  if ( boundary[2][0] && !neighbors.neighbors[1][1][0] ) z = -1;
2426  else if( boundary[2][1] && !neighbors.neighbors[1][1][2] ) z = 1;
2427 
2428  if( x || y || z )
2429  {
2430  // Corner case
2431  if( x && y && z ) flags[1+x][1+y][1+z] = true;
2432  // Edge cases
2433  if( x && y ) flags[1+x][1+y][1 ] = true;
2434  if( x && z ) flags[1+x][1 ][1+z] = true;
2435  if( y && z ) flags[1 ][1+y][1+1] = true;
2436  // Face cases
2437  if( x ) flags[1+x][1 ][1 ] = true;
2438  if( y ) flags[1 ][1+y][1 ] = true;
2439  if( z ) flags[1 ][1 ][1+z] = true;
2440  nKey.setNeighbors( leaf , flags );
2441  }
2442  }
2443  }
2444  _sNodes.set( tree );
2445  MemoryUsage();
2446  }
2447 
2448  template<int Degree>
2449  void Octree<Degree>::GetMCIsoTriangles( Real isoValue , int subdivideDepth , CoredMeshData* mesh , int fullDepthIso , int nonLinearFit , bool addBarycenter , bool polygonMesh )
2450  {
2451  fData.setValueTables( fData.VALUE_FLAG | fData.D_VALUE_FLAG , 0 , postNormalSmooth );
2452 
2453  // Ensure that the subtrees are self-contained
2454  RefineBoundary( subdivideDepth );
2455 
2456  RootData rootData , coarseRootData;
2457  std::vector< Point3D< float > >* interiorPoints;
2458  int maxDepth = tree.maxDepth();
2459 
2460  int sDepth = subdivideDepth<=0 ? 0 : std::max< int >( 0 , maxDepth-subdivideDepth );
2461 
2462  std::vector< Real > metSolution( _sNodes.nodeCount[maxDepth] , 0 );
2463 #pragma omp parallel for num_threads( threads )
2464  for( int i=_sNodes.nodeCount[_minDepth] ; i<_sNodes.nodeCount[maxDepth] ; i++ ) metSolution[i] = _sNodes.treeNodes[i]->nodeData.solution;
2465  for( int d=0 ; d<maxDepth ; d++ ) UpSample( d , _sNodes , &metSolution[0] );
2466 
2467  // Clear the marching cube indices
2468 #pragma omp parallel for num_threads( threads )
2469  for( int i=0 ; i<_sNodes.nodeCount[maxDepth+1] ; i++ ) _sNodes.treeNodes[i]->nodeData.mcIndex = 0;
2470 
2471  rootData.boundaryValues = new std::unordered_map< long long , std::pair< Real , Point3D< Real > > >();
2472  int offSet = 0;
2473 
2474  int maxCCount = _sNodes.getMaxCornerCount( &tree , sDepth , maxDepth , threads );
2475  int maxECount = _sNodes.getMaxEdgeCount ( &tree , sDepth , threads );
2476  rootData.cornerValues = new Real [ maxCCount ];
2477  rootData.cornerNormals = new Point3D< Real >[ maxCCount ];
2478  rootData.interiorRoots = new int [ maxECount ];
2479  rootData.cornerValuesSet = new char[ maxCCount ];
2480  rootData.cornerNormalsSet = new char[ maxCCount ];
2481  rootData.edgesSet = new char[ maxECount ];
2482  _sNodes.setCornerTable( coarseRootData , &tree , sDepth , threads );
2483  coarseRootData.cornerValues = new Real[ coarseRootData.cCount ];
2484  coarseRootData.cornerNormals = new Point3D< Real >[ coarseRootData.cCount ];
2485  coarseRootData.cornerValuesSet = new char[ coarseRootData.cCount ];
2486  coarseRootData.cornerNormalsSet = new char[ coarseRootData.cCount ];
2487  memset( coarseRootData.cornerValuesSet , 0 , sizeof( char ) * coarseRootData.cCount );
2488  memset( coarseRootData.cornerNormalsSet , 0 , sizeof( char ) * coarseRootData.cCount );
2489  MemoryUsage();
2490 
2491  std::vector< TreeOctNode::ConstNeighborKey3 > nKeys( threads );
2492  for( int t=0 ; t<threads ; t++ ) nKeys[t].set( maxDepth );
2493  TreeOctNode::ConstNeighborKey3 nKey;
2494  std::vector< TreeOctNode::ConstNeighborKey5 > nKeys5( threads );
2495  for( int t=0 ; t<threads ; t++ ) nKeys5[t].set( maxDepth );
2496  TreeOctNode::ConstNeighborKey5 nKey5;
2497  nKey5.set( maxDepth );
2498  nKey.set( maxDepth );
2499  // First process all leaf nodes at depths strictly finer than sDepth, one subtree at a time.
2500  for( int i=_sNodes.nodeCount[sDepth] ; i<_sNodes.nodeCount[sDepth+1] ; i++ )
2501  {
2502  if( !_sNodes.treeNodes[i]->children ) continue;
2503 
2504  _sNodes.setCornerTable( rootData , _sNodes.treeNodes[i] , threads );
2505  _sNodes.setEdgeTable ( rootData , _sNodes.treeNodes[i] , threads );
2506  memset( rootData.cornerValuesSet , 0 , sizeof( char ) * rootData.cCount );
2507  memset( rootData.cornerNormalsSet , 0 , sizeof( char ) * rootData.cCount );
2508  memset( rootData.edgesSet , 0 , sizeof( char ) * rootData.eCount );
2509  interiorPoints = new std::vector< Point3D< float > >();
2510  for( int d=maxDepth ; d>sDepth ; d-- )
2511  {
2512  int leafNodeCount = 0;
2513  std::vector< TreeOctNode* > leafNodes;
2514  for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodeCount++;
2515  leafNodes.reserve( leafNodeCount );
2516  for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodes.push_back( node );
2517  Stencil< double , 3 > stencil1[8] , stencil2[8][8];
2518  SetEvaluationStencils( d , stencil1 , stencil2 );
2519 
2520  // First set the corner values and associated marching-cube indices
2521 #pragma omp parallel for num_threads( threads )
2522  for( int t=0 ; t<threads ; t++ ) for( int i=(leafNodeCount*t)/threads ; i<(leafNodeCount*(t+1))/threads ; i++ )
2523  {
2524  TreeOctNode* leaf = leafNodes[i];
2525  SetIsoCorners( isoValue , leaf , rootData , rootData.cornerValuesSet , rootData.cornerValues , nKeys[t] , &metSolution[0] , stencil1 , stencil2 );
2526 
2527  // If this node shares a vertex with a coarser node, set the vertex value
2528  int d , off[3];
2529  leaf->depthAndOffset( d , off );
2530  int res = 1<<(d-sDepth);
2531  off[0] %= res , off[1] %=res , off[2] %= res;
2532  res--;
2533  if( !(off[0]%res) && !(off[1]%res) && !(off[2]%res) )
2534  {
2535  const TreeOctNode* temp = leaf;
2536  while( temp->d!=sDepth ) temp = temp->parent;
2537  int x = off[0]==0 ? 0 : 1 , y = off[1]==0 ? 0 : 1 , z = off[2]==0 ? 0 : 1;
2538  int c = Cube::CornerIndex( x , y , z );
2539  int idx = coarseRootData.cornerIndices( temp )[ c ];
2540  coarseRootData.cornerValues[ idx ] = rootData.cornerValues[ rootData.cornerIndices( leaf )[c] ];
2541  coarseRootData.cornerValuesSet[ idx ] = true;
2542  }
2543 
2544  // Compute the iso-vertices
2546  SetMCRootPositions( leaf , sDepth , isoValue , nKeys5[t] , rootData , interiorPoints , mesh , &metSolution[0] , nonLinearFit );
2547  }
2548  // Note that this should be broken off for multi-threading as
2549  // the SetMCRootPositions writes to interiorPoints (with lockupdateing)
2550  // while GetMCIsoTriangles reads from interiorPoints (without locking)
2551 #if MISHA_DEBUG
2552  std::vector< Point3D< float > > barycenters;
2553  std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? & barycenters : NULL;
2554 #endif // MISHA_DEBUG
2555 #pragma omp parallel for num_threads( threads )
2556  for( int t=0 ; t<threads ; t++ ) for( int i=(leafNodeCount*t)/threads ; i<(leafNodeCount*(t+1))/threads ; i++ )
2557  {
2558  TreeOctNode* leaf = leafNodes[i];
2560 #if MISHA_DEBUG
2561  GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , polygonMesh , barycenterPtr );
2562 #else // !MISHA_DEBUG
2563  GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , addBarycenter , polygonMesh );
2564 #endif // MISHA_DEBUG
2565  }
2566 #if MISHA_DEBUG
2567  for( int i=0 ; i<barycenters.size() ; i++ ) interiorPoints->push_back( barycenters[i] );
2568 #endif // MISHA_DEBUG
2569  }
2570  offSet = mesh->outOfCorePointCount();
2571 #if 1
2572  delete interiorPoints;
2573 #endif
2574  }
2575  MemoryUsage();
2576  delete[] rootData.cornerValues , delete[] rootData.cornerNormals , rootData.cornerValues = NULL , rootData.cornerNormals = NULL;
2577  delete[] rootData.cornerValuesSet , delete[] rootData.cornerNormalsSet , rootData.cornerValuesSet = NULL , rootData.cornerNormalsSet = NULL;
2578  delete[] rootData.interiorRoots ; rootData.interiorRoots = NULL;
2579  delete[] rootData.edgesSet ; rootData.edgesSet = NULL;
2580  coarseRootData.interiorRoots = NULL;
2581  coarseRootData.boundaryValues = rootData.boundaryValues;
2582  for( auto iter=rootData.boundaryRoots.cbegin() ; iter!=rootData.boundaryRoots.cend() ; iter++ )
2583  coarseRootData.boundaryRoots[iter->first] = iter->second;
2584 
2585  for( int d=sDepth ; d>=0 ; d-- )
2586  {
2587  Stencil< double , 3 > stencil1[8] , stencil2[8][8];
2588  SetEvaluationStencils( d , stencil1 , stencil2 );
2589 #if MISHA_DEBUG
2590  std::vector< Point3D< float > > barycenters;
2591  std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? &barycenters : NULL;
2592 #endif // MISHA_DEBUG
2593  for( int i=_sNodes.nodeCount[d] ; i<_sNodes.nodeCount[d+1] ; i++ )
2594  {
2595  TreeOctNode* leaf = _sNodes.treeNodes[i];
2596  if( leaf->children ) continue;
2597 
2598  // First set the corner values and associated marching-cube indices
2599  SetIsoCorners( isoValue , leaf , coarseRootData , coarseRootData.cornerValuesSet , coarseRootData.cornerValues , nKey , &metSolution[0] , stencil1 , stencil2 );
2600 
2601  // Now compute the iso-vertices
2603  {
2604  SetMCRootPositions( leaf , 0 , isoValue , nKey5 , coarseRootData , NULL , mesh , &metSolution[0] , nonLinearFit );
2605 #if MISHA_DEBUG
2606  GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , polygonMesh , barycenterPtr );
2607 #else // !MISHA_DEBUG
2608  GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , addBarycenter , polygonMesh );
2609 #endif // MISHA_DEBUG
2610  }
2611  }
2612  }
2613  MemoryUsage();
2614 
2615  delete[] coarseRootData.cornerValues , delete[] coarseRootData.cornerNormals;
2616  delete[] coarseRootData.cornerValuesSet , delete[] coarseRootData.cornerNormalsSet;
2617  delete rootData.boundaryValues;
2618  }
2619 
2620  template<int Degree>
2622  int idx[3];
2623  Real value=0;
2624 
2625  VertexData::CenterIndex(node,fData.depth,idx);
2626  idx[0]*=fData.functionCount;
2627  idx[1]*=fData.functionCount;
2628  idx[2]*=fData.functionCount;
2629  int minDepth = std::max< int >( 0 , std::min< int >( _minDepth , node->depth()-1 ) );
2630  for( int i=minDepth ; i<=node->depth() ; i++ )
2631  for(int j=0;j<3;j++)
2632  for(int k=0;k<3;k++)
2633  for(int l=0;l<3;l++)
2634  {
2635  const TreeOctNode* n=neighborKey.neighbors[i].neighbors[j][k][l];
2636  if( n )
2637  {
2638  Real temp=n->nodeData.solution;
2639  value+=temp*Real(
2640  fData.valueTables[idx[0]+int(n->off[0])]*
2641  fData.valueTables[idx[1]+int(n->off[1])]*
2642  fData.valueTables[idx[2]+int(n->off[2])]);
2643  }
2644  }
2645  if(node->children)
2646  {
2647  for(int i=0;i<Cube::CORNERS;i++){
2648  int ii=Cube::AntipodalCornerIndex(i);
2649  const TreeOctNode* n=&node->children[i];
2650  while(1){
2651  value+=n->nodeData.solution*Real(
2652  fData.valueTables[idx[0]+int(n->off[0])]*
2653  fData.valueTables[idx[1]+int(n->off[1])]*
2654  fData.valueTables[idx[2]+int(n->off[2])]);
2655  if( n->children ) n=&n->children[ii];
2656  else break;
2657  }
2658  }
2659  }
2660  return value;
2661  }
2662 
2663  template< int Degree >
2664  Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution )
2665  {
2666  int idx[3];
2667  double value = 0;
2668 
2669  VertexData::CornerIndex( node , corner , fData.depth , idx );
2670  idx[0] *= fData.functionCount;
2671  idx[1] *= fData.functionCount;
2672  idx[2] *= fData.functionCount;
2673 
2674  int d = node->depth();
2675  int cx , cy , cz;
2676  int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3;
2677  Cube::FactorCornerIndex( corner , cx , cy , cz );
2678  {
2679  TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d];
2680  if( cx==0 ) endX = 2;
2681  else startX = 1;
2682  if( cy==0 ) endY = 2;
2683  else startY = 1;
2684  if( cz==0 ) endZ = 2;
2685  else startZ = 1;
2686  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2687  {
2688  const TreeOctNode* n=neighbors.neighbors[x][y][z];
2689  if( n )
2690  {
2691  double v =
2692  fData.valueTables[ idx[0]+int(n->off[0]) ]*
2693  fData.valueTables[ idx[1]+int(n->off[1]) ]*
2694  fData.valueTables[ idx[2]+int(n->off[2]) ];
2695  value += n->nodeData.solution * v;
2696  }
2697  }
2698  }
2699  if( d>0 && d>_minDepth )
2700  {
2701  int _corner = int( node - node->parent->children );
2702  int _cx , _cy , _cz;
2703  Cube::FactorCornerIndex( _corner , _cx , _cy , _cz );
2704  if( cx!=_cx ) startX = 0 , endX = 3;
2705  if( cy!=_cy ) startY = 0 , endY = 3;
2706  if( cz!=_cz ) startZ = 0 , endZ = 3;
2707  TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1];
2708  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2709  {
2710  const TreeOctNode* n=neighbors.neighbors[x][y][z];
2711  if( n )
2712  {
2713  double v =
2714  fData.valueTables[ idx[0]+int(n->off[0]) ]*
2715  fData.valueTables[ idx[1]+int(n->off[1]) ]*
2716  fData.valueTables[ idx[2]+int(n->off[2]) ];
2717  value += metSolution[ n->nodeData.nodeIndex ] * v;
2718  }
2719  }
2720  }
2721  return Real( value );
2722  }
2723 
2724  template< int Degree >
2725  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] )
2726  {
2727  double value = 0;
2728  int d = node->depth();
2729  int cx , cy , cz;
2730  int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3;
2731  Cube::FactorCornerIndex( corner , cx , cy , cz );
2732  {
2733  TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d];
2734  if( cx==0 ) endX = 2;
2735  else startX = 1;
2736  if( cy==0 ) endY = 2;
2737  else startY = 1;
2738  if( cz==0 ) endZ = 2;
2739  else startZ = 1;
2740  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2741  {
2742  const TreeOctNode* n=neighbors.neighbors[x][y][z];
2743  if( n ) value += n->nodeData.solution * stencil1[x][y][z];
2744  }
2745  }
2746  if( d>0 && d>_minDepth )
2747  {
2748  int _corner = int( node - node->parent->children );
2749  int _cx , _cy , _cz;
2750  Cube::FactorCornerIndex( _corner , _cx , _cy , _cz );
2751  if( cx!=_cx ) startX = 0 , endX = 3;
2752  if( cy!=_cy ) startY = 0 , endY = 3;
2753  if( cz!=_cz ) startZ = 0 , endZ = 3;
2754  TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1];
2755  for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
2756  {
2757  const TreeOctNode* n=neighbors.neighbors[x][y][z];
2758  if( n ) value += metSolution[ n->nodeData.nodeIndex ] * stencil2[x][y][z];
2759  }
2760  }
2761  return Real( value );
2762  }
2763 
2764  template< int Degree >
2765  Point3D< Real > Octree< Degree >::getCornerNormal( const OctNode< TreeNodeData , Real >::ConstNeighborKey5& neighborKey5 , const TreeOctNode* node , int corner , const Real* metSolution )
2766  {
2767  int idx[3];
2768  Point3D< Real > normal;
2769  normal[0] = normal[1] = normal[2] = 0.;
2770 
2771  VertexData::CornerIndex( node , corner , fData.depth , idx );
2772  idx[0] *= fData.functionCount;
2773  idx[1] *= fData.functionCount;
2774  idx[2] *= fData.functionCount;
2775 
2776  int d = node->depth();
2777  // Iterate over all ancestors that can overlap the corner
2778  {
2779  TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d];
2780  for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ )
2781  {
2782  const TreeOctNode* n=neighbors.neighbors[j][k][l];
2783  if( n )
2784  {
2785  int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] };
2786  double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] };
2787  double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] };
2788  Real solution = n->nodeData.solution;
2789  normal[0] += Real( dValues[0] * values[1] * values[2] * solution );
2790  normal[1] += Real( values[0] * dValues[1] * values[2] * solution );
2791  normal[2] += Real( values[0] * values[1] * dValues[2] * solution );
2792  }
2793  }
2794  }
2795  if( d>0 && d>_minDepth )
2796  {
2797  TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d-1];
2798  for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ )
2799  {
2800  const TreeOctNode* n=neighbors.neighbors[j][k][l];
2801  if( n )
2802  {
2803  int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] };
2804  double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] };
2805  double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] };
2806  Real solution = metSolution[ n->nodeData.nodeIndex ];
2807  normal[0] += Real( dValues[0] * values[1] * values[2] * solution );
2808  normal[1] += Real( values[0] * dValues[1] * values[2] * solution );
2809  normal[2] += Real( values[0] * values[1] * dValues[2] * solution );
2810  }
2811  }
2812  }
2813  return normal;
2814  }
2815 
2816  template< int Degree >
2818  {
2819  Real isoValue , weightSum;
2820 
2821  neighborKey2.set( fData.depth );
2822  fData.setValueTables( fData.VALUE_FLAG , 0 );
2823 
2824  isoValue = weightSum = 0;
2825 #pragma omp parallel for num_threads( threads ) reduction( + : isoValue , weightSum )
2826  for( int t=0 ; t<threads ; t++)
2827  {
2828  TreeOctNode::ConstNeighborKey3 nKey;
2829  nKey.set( _sNodes.maxDepth-1 );
2830  int nodeCount = _sNodes.nodeCount[ _sNodes.maxDepth ];
2831  for( int i=(nodeCount*t)/threads ; i<(nodeCount*(t+1))/threads ; i++ )
2832  {
2833  TreeOctNode* temp = _sNodes.treeNodes[i];
2834  nKey.getNeighbors( temp );
2836  if( w!=0 )
2837  {
2838  isoValue += getCenterValue( nKey , temp ) * w;
2839  weightSum += w;
2840  }
2841  }
2842  }
2843  return isoValue/weightSum;
2844  }
2845 
2846  template< int Degree >
2847  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] )
2848  {
2849  Real cornerValues[ Cube::CORNERS ];
2850  const SortedTreeNodes::CornerIndices& cIndices = cData[ leaf ];
2851 
2852  bool isInterior;
2853  int d , off[3];
2854  leaf->depthAndOffset( d , off );
2855  int mn = 2 , mx = (1<<d)-2;
2856  isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
2857  nKey.getNeighbors( leaf );
2858  for( int c=0 ; c<Cube::CORNERS ; c++ )
2859  {
2860  int vIndex = cIndices[c];
2861  if( valuesSet[vIndex] ) cornerValues[c] = values[vIndex];
2862  else
2863  {
2864  if( isInterior ) cornerValues[c] = getCornerValue( nKey , leaf , c , metSolution , stencil1[c].values , stencil2[int(leaf - leaf->parent->children)][c].values );
2865  else cornerValues[c] = getCornerValue( nKey , leaf , c , metSolution );
2866  values[vIndex] = cornerValues[c];
2867  valuesSet[vIndex] = 1;
2868  }
2869  }
2870 
2871  leaf->nodeData.mcIndex = MarchingCubes::GetIndex( cornerValues , isoValue );
2872 
2873  // Set the marching cube indices for all interior nodes.
2874  if( leaf->parent )
2875  {
2876  TreeOctNode* parent = leaf->parent;
2877  int c = int( leaf - leaf->parent->children );
2878  int mcid = leaf->nodeData.mcIndex & (1<<MarchingCubes::cornerMap()[c]);
2879 
2880  if( mcid )
2881  {
2882  poisson::atomicOr(parent->nodeData.mcIndex, mcid);
2883 
2884  while( 1 )
2885  {
2886  if( parent->parent && parent->parent->d>=_minDepth && (parent-parent->parent->children)==c )
2887  {
2888  poisson::atomicOr(parent->parent->nodeData.mcIndex, mcid);
2889  parent = parent->parent;
2890  }
2891  else break;
2892  }
2893  }
2894  }
2895  }
2896 
2897  template<int Degree>
2898  int Octree<Degree>::InteriorFaceRootCount(const TreeOctNode* node,const int &faceIndex,int maxDepth){
2899  int c1,c2,e1,e2,dir,off,cnt=0;
2900  int corners[Cube::CORNERS/2];
2901  if(node->children){
2902  Cube::FaceCorners(faceIndex,corners[0],corners[1],corners[2],corners[3]);
2903  Cube::FactorFaceIndex(faceIndex,dir,off);
2904  c1=corners[0];
2905  c2=corners[3];
2906  switch(dir){
2907  case 0:
2908  e1=Cube::EdgeIndex(1,off,1);
2909  e2=Cube::EdgeIndex(2,off,1);
2910  break;
2911  case 1:
2912  e1=Cube::EdgeIndex(0,off,1);
2913  e2=Cube::EdgeIndex(2,1,off);
2914  break;
2915  case 2:
2916  e1=Cube::EdgeIndex(0,1,off);
2917  e2=Cube::EdgeIndex(1,1,off);
2918  break;
2919  };
2920  cnt+=EdgeRootCount(&node->children[c1],e1,maxDepth)+EdgeRootCount(&node->children[c1],e2,maxDepth);
2921  switch(dir){
2922  case 0:
2923  e1=Cube::EdgeIndex(1,off,0);
2924  e2=Cube::EdgeIndex(2,off,0);
2925  break;
2926  case 1:
2927  e1=Cube::EdgeIndex(0,off,0);
2928  e2=Cube::EdgeIndex(2,0,off);
2929  break;
2930  case 2:
2931  e1=Cube::EdgeIndex(0,0,off);
2932  e2=Cube::EdgeIndex(1,0,off);
2933  break;
2934  };
2935  cnt+=EdgeRootCount(&node->children[c2],e1,maxDepth)+EdgeRootCount(&node->children[c2],e2,maxDepth);
2936  for(int i=0;i<Cube::CORNERS/2;i++){if(node->children[corners[i]].children){cnt+=InteriorFaceRootCount(&node->children[corners[i]],faceIndex,maxDepth);}}
2937  }
2938  return cnt;
2939  }
2940 
2941  template<int Degree>
2942  int Octree<Degree>::EdgeRootCount(const TreeOctNode* node,int edgeIndex,int maxDepth){
2943  int f1,f2,c1,c2;
2944  const TreeOctNode* temp;
2945  Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);
2946 
2947  int eIndex;
2948  const TreeOctNode* finest=node;
2949  eIndex=edgeIndex;
2950  if(node->depth()<maxDepth){
2951  temp=node->faceNeighbor(f1);
2952  if(temp && temp->children){
2953  finest=temp;
2954  eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1);
2955  }
2956  else{
2957  temp=node->faceNeighbor(f2);
2958  if(temp && temp->children){
2959  finest=temp;
2960  eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2);
2961  }
2962  else{
2963  temp=node->edgeNeighbor(edgeIndex);
2964  if(temp && temp->children){
2965  finest=temp;
2966  eIndex=Cube::EdgeReflectEdgeIndex(edgeIndex);
2967  }
2968  }
2969  }
2970  }
2971 
2972  Cube::EdgeCorners(eIndex,c1,c2);
2973  if(finest->children) return EdgeRootCount(&finest->children[c1],eIndex,maxDepth)+EdgeRootCount(&finest->children[c2],eIndex,maxDepth);
2974  else return MarchingCubes::HasEdgeRoots(finest->nodeData.mcIndex,eIndex);
2975  }
2976 
2977  template<int Degree>
2978  int Octree<Degree>::IsBoundaryFace(const TreeOctNode* node,int faceIndex,int subdivideDepth){
2979  int dir,offset,d,o[3],idx;
2980 
2981  if(subdivideDepth<0){return 0;}
2982  if(node->d<=subdivideDepth){return 1;}
2983  Cube::FactorFaceIndex(faceIndex,dir,offset);
2984  node->depthAndOffset(d,o);
2985 
2986  idx=(int(o[dir])<<1) + (offset<<1);
2987  return !(idx%(2<<(int(node->d)-subdivideDepth)));
2988  }
2989 
2990  template<int Degree>
2991  int Octree<Degree>::IsBoundaryEdge(const TreeOctNode* node,int edgeIndex,int subdivideDepth){
2992  int dir,x,y;
2993  Cube::FactorEdgeIndex(edgeIndex,dir,x,y);
2994  return IsBoundaryEdge(node,dir,x,y,subdivideDepth);
2995  }
2996 
2997  template<int Degree>
2998  int Octree<Degree>::IsBoundaryEdge( const TreeOctNode* node , int dir , int x , int y , int subdivideDepth )
2999  {
3000  int d , o[3] , idx1 , idx2 , mask;
3001 
3002  if( subdivideDepth<0 ) return 0;
3003  if( node->d<=subdivideDepth ) return 1;
3004  node->depthAndOffset( d , o );
3005 
3006  switch( dir )
3007  {
3008  case 0:
3009  idx1 = o[1] + x;
3010  idx2 = o[2] + y;
3011  break;
3012  case 1:
3013  idx1 = o[0] + x;
3014  idx2 = o[2] + y;
3015  break;
3016  case 2:
3017  idx1 = o[0] + x;
3018  idx2 = o[1] + y;
3019  break;
3020  }
3021  mask = 1<<( int(node->d) - subdivideDepth );
3022  return !(idx1%(mask)) || !(idx2%(mask));
3023  }
3024 
3025  template< int Degree >
3027  {
3028  int o , i1 , i2;
3029  Real width;
3030  Point3D< Real > c;
3031 
3032  Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 );
3033  ri.node->centerAndWidth( c , width );
3034  switch(o)
3035  {
3036  case 0:
3037  start[0] = c[0] - width/2;
3038  end [0] = c[0] + width/2;
3039  start[1] = end[1] = c[1] - width/2 + width*i1;
3040  start[2] = end[2] = c[2] - width/2 + width*i2;
3041  break;
3042  case 1:
3043  start[0] = end[0] = c[0] - width/2 + width*i1;
3044  start[1] = c[1] - width/2;
3045  end [1] = c[1] + width/2;
3046  start[2] = end[2] = c[2] - width/2 + width*i2;
3047  break;
3048  case 2:
3049  start[0] = end[0] = c[0] - width/2 + width*i1;
3050  start[1] = end[1] = c[1] - width/2 + width*i2;
3051  start[2] = c[2] - width/2;
3052  end [2] = c[2] + width/2;
3053  break;
3054  }
3055  }
3056 
3057  //////////////////////////////////////////////////////////////////////////////////////
3058  // The assumption made when calling this code is that the edge has at most one root //
3059  //////////////////////////////////////////////////////////////////////////////////////
3060  template< int Degree >
3061  int Octree< Degree >::GetRoot( const RootInfo& ri , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , Point3D< Real > & position , RootData& rootData , int sDepth , const Real* metSolution , int nonLinearFit )
3062  {
3063  if( !MarchingCubes::HasRoots( ri.node->nodeData.mcIndex ) ) return 0;
3064  int c1 , c2;
3065  Cube::EdgeCorners( ri.edgeIndex , c1 , c2 );
3066  if( !MarchingCubes::HasEdgeRoots( ri.node->nodeData.mcIndex , ri.edgeIndex ) ) return 0;
3067 
3068  long long key1 , key2;
3069  Point3D< Real > n[2];
3070 
3071  int i , o , i1 , i2 , rCount=0;
3072  Polynomial<2> P;
3073  std::vector< double > roots;
3074  double x0 , x1;
3075  Real center , width;
3076  Real averageRoot=0;
3077  Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 );
3078  int idx1[3] , idx2[3];
3079  key1 = VertexData::CornerIndex( ri.node , c1 , fData.depth , idx1 );
3080  key2 = VertexData::CornerIndex( ri.node , c2 , fData.depth , idx2 );
3081 
3082  bool isBoundary = ( IsBoundaryEdge( ri.node , ri.edgeIndex , sDepth )!=0 );
3083  bool haveKey1 , haveKey2;
3084  std::pair< Real , Point3D< Real > > keyValue1 , keyValue2;
3085  int iter1 , iter2;
3086  {
3087  iter1 = rootData.cornerIndices( ri.node )[ c1 ];
3088  iter2 = rootData.cornerIndices( ri.node )[ c2 ];
3089  keyValue1.first = rootData.cornerValues[iter1];
3090  keyValue2.first = rootData.cornerValues[iter2];
3091  if( isBoundary )
3092  {
3093 #pragma omp critical (normal_hash_access)
3094  {
3095  haveKey1 = ( rootData.boundaryValues->count( key1 )>0 );
3096  haveKey2 = ( rootData.boundaryValues->count( key2 )>0 );
3097  if( haveKey1 ) keyValue1 = (*rootData.boundaryValues)[key1];
3098  if( haveKey2 ) keyValue2 = (*rootData.boundaryValues)[key2];
3099  }
3100  }
3101  else
3102  {
3103  haveKey1 = ( rootData.cornerNormalsSet[ iter1 ] != 0 );
3104  haveKey2 = ( rootData.cornerNormalsSet[ iter2 ] != 0 );
3105  keyValue1.first = rootData.cornerValues[iter1];
3106  keyValue2.first = rootData.cornerValues[iter2];
3107  if( haveKey1 ) keyValue1.second = rootData.cornerNormals[iter1];
3108  if( haveKey2 ) keyValue2.second = rootData.cornerNormals[iter2];
3109  }
3110  }
3111  if( !haveKey1 || !haveKey2 ) neighborKey5.getNeighbors( ri.node );
3112  if( !haveKey1 ) keyValue1.second = getCornerNormal( neighborKey5 , ri.node , c1 , metSolution );
3113  x0 = keyValue1.first;
3114  n[0] = keyValue1.second;
3115 
3116  if( !haveKey2 ) keyValue2.second = getCornerNormal( neighborKey5 , ri.node , c2 , metSolution );
3117  x1 = keyValue2.first;
3118  n[1] = keyValue2.second;
3119 
3120  if( !haveKey1 || !haveKey2 )
3121  {
3122  if( isBoundary )
3123  {
3124 #pragma omp critical (normal_hash_access)
3125  {
3126  if( !haveKey1 ) (*rootData.boundaryValues)[key1] = keyValue1;
3127  if( !haveKey2 ) (*rootData.boundaryValues)[key2] = keyValue2;
3128  }
3129  }
3130  else
3131  {
3132  if( !haveKey1 ) rootData.cornerNormals[iter1] = keyValue1.second , rootData.cornerNormalsSet[ iter1 ] = 1;
3133  if( !haveKey2 ) rootData.cornerNormals[iter2] = keyValue2.second , rootData.cornerNormalsSet[ iter2 ] = 1;
3134  }
3135  }
3136 
3137  Point3D< Real > c;
3138  ri.node->centerAndWidth(c,width);
3139  center=c[o];
3140  for( i=0 ; i<DIMENSION ; i++ ) n[0][i] *= width , n[1][i] *= width;
3141 
3142  switch(o)
3143  {
3144  case 0:
3145  position[1] = c[1]-width/2+width*i1;
3146  position[2] = c[2]-width/2+width*i2;
3147  break;
3148  case 1:
3149  position[0] = c[0]-width/2+width*i1;
3150  position[2] = c[2]-width/2+width*i2;
3151  break;
3152  case 2:
3153  position[0] = c[0]-width/2+width*i1;
3154  position[1] = c[1]-width/2+width*i2;
3155  break;
3156  }
3157  double dx0,dx1;
3158  dx0 = n[0][o];
3159  dx1 = n[1][o];
3160 
3161  // The scaling will turn the Hermite Spline into a quadratic
3162  double scl=(x1-x0)/((dx1+dx0)/2);
3163  dx0 *= scl;
3164  dx1 *= scl;
3165 
3166  // Hermite Spline
3167  P.coefficients[0] = x0;
3168  P.coefficients[1] = dx0;
3169  P.coefficients[2] = 3*(x1-x0)-dx1-2*dx0;
3170 
3171  P.getSolutions( isoValue , roots , EPSILON );
3172  for( i=0 ; i<int(roots.size()) ; i++ )
3173  if( roots[i]>=0 && roots[i]<=1 )
3174  {
3175  averageRoot += Real( roots[i] );
3176  rCount++;
3177  }
3178  if( rCount && nonLinearFit ) averageRoot /= rCount;
3179  else averageRoot = Real((x0-isoValue)/(x0-x1));
3180  if( averageRoot<0 || averageRoot>1 )
3181  {
3182  fprintf( stderr , "[WARNING] Bad average root: %f\n" , averageRoot );
3183  fprintf( stderr , "\t(%f %f) , (%f %f) (%f)\n" , x0 , x1 , dx0 , dx1 , isoValue );
3184  if( averageRoot<0 ) averageRoot = 0;
3185  if( averageRoot>1 ) averageRoot = 1;
3186  }
3187  position[o] = Real(center-width/2+width*averageRoot);
3188  return 1;
3189  }
3190 
3191  template< int Degree >
3192  int Octree< Degree >::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , int sDepth,RootInfo& ri )
3193  {
3194  int c1,c2,f1,f2;
3195  const TreeOctNode *temp,*finest;
3196  int finestIndex;
3197 
3198  Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);
3199 
3200  finest=node;
3201  finestIndex=edgeIndex;
3202  if(node->depth()<maxDepth){
3203  if(IsBoundaryFace(node,f1,sDepth)){temp=NULL;}
3204  else{temp=node->faceNeighbor(f1);}
3205  if(temp && temp->children){
3206  finest=temp;
3207  finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1);
3208  }
3209  else{
3210  if(IsBoundaryFace(node,f2,sDepth)){temp=NULL;}
3211  else{temp=node->faceNeighbor(f2);}
3212  if(temp && temp->children){
3213  finest=temp;
3214  finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2);
3215  }
3216  else{
3217  if(IsBoundaryEdge(node,edgeIndex,sDepth)){temp=NULL;}
3218  else{temp=node->edgeNeighbor(edgeIndex);}
3219  if(temp && temp->children){
3220  finest=temp;
3221  finestIndex=Cube::EdgeReflectEdgeIndex(edgeIndex);
3222  }
3223  }
3224  }
3225  }
3226 
3227  Cube::EdgeCorners(finestIndex,c1,c2);
3228  if(finest->children){
3229  if (GetRootIndex(&finest->children[c1],finestIndex,maxDepth,sDepth,ri)) {return 1;}
3230  else if (GetRootIndex(&finest->children[c2],finestIndex,maxDepth,sDepth,ri)) {return 1;}
3231  else
3232  {
3233  fprintf( stderr , "[WARNING] Couldn't find root index with either child\n" );
3234  return 0;
3235  }
3236  }
3237  else
3238  {
3239  if( !(MarchingCubes::edgeMask()[finest->nodeData.mcIndex] & (1<<finestIndex)) )
3240  {
3241  fprintf( stderr , "[WARNING] Finest node does not have iso-edge\n" );
3242  return 0;
3243  }
3244 
3245  int o,i1,i2;
3246  Cube::FactorEdgeIndex(finestIndex,o,i1,i2);
3247  int d,off[3];
3248  finest->depthAndOffset(d,off);
3249  ri.node=finest;
3250  ri.edgeIndex=finestIndex;
3251  int eIndex[2],offset;
3252  offset=BinaryNode<Real>::Index( d , off[o] );
3253  switch(o)
3254  {
3255  case 0:
3256  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
3257  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3258  break;
3259  case 1:
3260  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3261  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3262  break;
3263  case 2:
3264  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3265  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
3266  break;
3267  }
3268  ri.key = (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45;
3269  return 1;
3270  }
3271  }
3272 
3273  template<int Degree>
3274  int Octree<Degree>::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , RootInfo& ri )
3275  {
3276  int c1,c2,f1,f2;
3277  const TreeOctNode *temp,*finest;
3278  int finestIndex;
3279 
3280 
3281  // The assumption is that the super-edge has a root along it.
3282  if(!(MarchingCubes::edgeMask()[node->nodeData.mcIndex] & (1<<edgeIndex))){return 0;}
3283 
3284  Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);
3285 
3286  finest = node;
3287  finestIndex = edgeIndex;
3288  if( node->depth()<maxDepth && !node->children )
3289  {
3290  temp=node->faceNeighbor( f1 );
3291  if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f1 );
3292  else
3293  {
3294  temp = node->faceNeighbor( f2 );
3295  if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f2 );
3296  else
3297  {
3298  temp = node->edgeNeighbor( edgeIndex );
3299  if( temp && temp->children ) finest = temp , finestIndex = Cube::EdgeReflectEdgeIndex( edgeIndex );
3300  }
3301  }
3302  }
3303 
3304  Cube::EdgeCorners( finestIndex , c1 , c2 );
3305  if( finest->children )
3306  {
3307  if ( GetRootIndex( finest->children + c1 , finestIndex , maxDepth , ri ) ) return 1;
3308  else if( GetRootIndex( finest->children + c2 , finestIndex , maxDepth , ri ) ) return 1;
3309  else
3310  {
3311  int d1 , off1[3] , d2 , off2[3];
3312  node->depthAndOffset( d1 , off1 );
3313  finest->depthAndOffset( d2 , off2 );
3314  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 );
3315  printf( "\t" );
3316  for( int i=0 ; i<8 ; i++ ) if( node->nodeData.mcIndex & (1<<i) ) printf( "1" ); else printf( "0" );
3317  printf( "\t" );
3318  for( int i=0 ; i<8 ; i++ ) if( finest->nodeData.mcIndex & (1<<i) ) printf( "1" ); else printf( "0" );
3319  printf( "\n" );
3320  return 0;
3321  }
3322  }
3323  else
3324  {
3325  int o,i1,i2;
3326  Cube::FactorEdgeIndex(finestIndex,o,i1,i2);
3327  int d,off[3];
3328  finest->depthAndOffset(d,off);
3329  ri.node=finest;
3330  ri.edgeIndex=finestIndex;
3331  int offset,eIndex[2];
3332  offset = BinaryNode< Real >::CenterIndex( d , off[o] );
3333  switch(o){
3334  case 0:
3335  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
3336  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3337  break;
3338  case 1:
3339  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3340  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3341  break;
3342  case 2:
3343  eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3344  eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
3345  break;
3346  }
3347  ri.key= (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45;
3348  return 1;
3349  }
3350  }
3351 
3352  template<int Degree>
3353  int Octree<Degree>::GetRootPair(const RootInfo& ri,int maxDepth,RootInfo& pair){
3354  const TreeOctNode* node=ri.node;
3355  int c1,c2,c;
3356  Cube::EdgeCorners(ri.edgeIndex,c1,c2);
3357  while(node->parent){
3358  c=int(node-node->parent->children);
3359  if(c!=c1 && c!=c2){return 0;}
3360  if(!MarchingCubes::HasEdgeRoots(node->parent->nodeData.mcIndex,ri.edgeIndex)){
3361  if(c==c1){return GetRootIndex(&node->parent->children[c2],ri.edgeIndex,maxDepth,pair);}
3362  else{return GetRootIndex(&node->parent->children[c1],ri.edgeIndex,maxDepth,pair);}
3363  }
3364  node=node->parent;
3365  }
3366  return 0;
3367  }
3368 
3369  template<int Degree>
3370  int Octree< Degree >::GetRootIndex( const RootInfo& ri , RootData& rootData , CoredPointIndex& index )
3371  {
3372  long long key = ri.key;
3373  auto rootIter = rootData.boundaryRoots.find( key );
3374  if( rootIter!=rootData.boundaryRoots.end() )
3375  {
3376  index.inCore = 1;
3377  index.index = rootIter->second;
3378  return 1;
3379  }
3380  else if( rootData.interiorRoots )
3381  {
3382  int eIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ];
3383  if( rootData.edgesSet[ eIndex ] )
3384  {
3385  index.inCore = 0;
3386  index.index = rootData.interiorRoots[ eIndex ];
3387  return 1;
3388  }
3389  }
3390  return 0;
3391  }
3392 
3393  template< int Degree >
3394  int Octree< Degree >::SetMCRootPositions( TreeOctNode* node , int sDepth , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , RootData& rootData ,
3395  std::vector< Point3D< float > >* interiorPositions , CoredMeshData* mesh , const Real* metSolution , int nonLinearFit )
3396  {
3397  Point3D< Real > position;
3398  int eIndex;
3399  RootInfo ri;
3400  int count=0;
3401  if( !MarchingCubes::HasRoots( node->nodeData.mcIndex ) ) return 0;
3402  for( int i=0 ; i<DIMENSION ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
3403  {
3404  long long key;
3405  eIndex = Cube::EdgeIndex( i , j , k );
3406  if( GetRootIndex( node , eIndex , fData.depth , ri ) )
3407  {
3408  key = ri.key;
3409  if( !rootData.interiorRoots || IsBoundaryEdge( node , i , j , k , sDepth ) )
3410  {
3411  std::unordered_map< long long , int >::iterator iter , end;
3412  // Check if the root has already been set
3413 #pragma omp critical (boundary_roots_hash_access)
3414  {
3415  iter = rootData.boundaryRoots.find( key );
3416  end = rootData.boundaryRoots.end();
3417  }
3418  if( iter==end )
3419  {
3420  // Get the root information
3421  GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit );
3422  // Add the root if it hasn't been added already
3423 #pragma omp critical (boundary_roots_hash_access)
3424  {
3425  iter = rootData.boundaryRoots.find( key );
3426  end = rootData.boundaryRoots.end();
3427  if( iter==end )
3428  {
3429  mesh->inCorePoints.push_back( position );
3430  rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() ) - 1;
3431  }
3432  }
3433  if( iter==end ) count++;
3434  }
3435  }
3436  else
3437  {
3438  int nodeEdgeIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ];
3439  if( !rootData.edgesSet[ nodeEdgeIndex ] )
3440  {
3441  // Get the root information
3442  GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit );
3443  // Add the root if it hasn't been added already
3444 #pragma omp critical (add_point_access)
3445  {
3446  if( !rootData.edgesSet[ nodeEdgeIndex ] )
3447  {
3448  rootData.interiorRoots[ nodeEdgeIndex ] = mesh->addOutOfCorePoint( position );
3449  interiorPositions->push_back( position );
3450  rootData.edgesSet[ nodeEdgeIndex ] = 1;
3451  count++;
3452  }
3453  }
3454  }
3455  }
3456  }
3457  }
3458  return count;
3459  }
3460 
3461  template<int Degree>
3462  int Octree< Degree >::SetBoundaryMCRootPositions( int sDepth , Real isoValue , RootData& rootData , CoredMeshData* mesh , int nonLinearFit )
3463  {
3464  Point3D< Real > position;
3465  int i,j,k,eIndex,hits=0;
3466  RootInfo ri;
3467  int count=0;
3468  TreeOctNode* node;
3469 
3470  node = tree.nextLeaf();
3471  while( node )
3472  {
3474  {
3475  hits=0;
3476  for( i=0 ; i<DIMENSION ; i++ )
3477  for( j=0 ; j<2 ; j++ )
3478  for( k=0 ; k<2 ; k++ )
3479  if( IsBoundaryEdge( node , i , j , k , sDepth ) )
3480  {
3481  hits++;
3482  long long key;
3483  eIndex = Cube::EdgeIndex( i , j , k );
3484  if( GetRootIndex( node , eIndex , fData.depth , ri ) )
3485  {
3486  key = ri.key;
3487  if( rootData.boundaryRoots.find(key)==rootData.boundaryRoots.end() )
3488  {
3489  GetRoot( ri , isoValue , position , rootData , sDepth , nonLinearFit );
3490  mesh->inCorePoints.push_back( position );
3491  rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() )-1;
3492  count++;
3493  }
3494  }
3495  }
3496  }
3497  if( hits ) node=tree.nextLeaf(node);
3498  else node=tree.nextBranch(node);
3499  }
3500  return count;
3501  }
3502 
3503  template<int Degree>
3504  void Octree< Degree >::GetMCIsoEdges( TreeOctNode* node , int sDepth , std::vector< std::pair< RootInfo , RootInfo > >& edges )
3505  {
3506  TreeOctNode* temp;
3507  int count=0 , tris=0;
3508  int isoTri[ DIMENSION * MarchingCubes::MAX_TRIANGLES ];
3509  FaceEdgesFunction fef;
3510  int ref , fIndex;
3511  std::unordered_map< long long , std::pair< RootInfo , int > > vertexCount;
3512 
3513  fef.edges = &edges;
3514  fef.maxDepth = fData.depth;
3515  fef.vertexCount = &vertexCount;
3516  count = MarchingCubes::AddTriangleIndices( node->nodeData.mcIndex , isoTri );
3517  for( fIndex=0 ; fIndex<Cube::NEIGHBORS ; fIndex++ )
3518  {
3519  ref = Cube::FaceReflectFaceIndex( fIndex , fIndex );
3520  fef.fIndex = ref;
3521  temp = node->faceNeighbor( fIndex );
3522  // If the face neighbor exists and has higher resolution than the current node,
3523  // get the iso-curve from the neighbor
3524  if( temp && temp->children && !IsBoundaryFace( node , fIndex , sDepth ) ) temp->processNodeFaces( temp , &fef , ref );
3525  // Otherwise, get it from the node
3526  else
3527  {
3528  RootInfo ri1 , ri2;
3529  for( int j=0 ; j<count ; j++ )
3530  for( int k=0 ; k<3 ; k++ )
3531  if( fIndex==Cube::FaceAdjacentToEdges( isoTri[j*3+k] , isoTri[j*3+((k+1)%3)] ) )
3532  if( GetRootIndex( node , isoTri[j*3+k] , fData.depth , ri1 ) && GetRootIndex( node , isoTri[j*3+((k+1)%3)] , fData.depth , ri2 ) )
3533  {
3534  long long key1 = ri1.key , key2 = ri2.key;
3535  edges.push_back( std::pair< RootInfo , RootInfo >( ri1 , ri2 ) );
3536  if( vertexCount.count( key1 )==0 )
3537  {
3538  vertexCount[key1].first = ri1;
3539  vertexCount[key1].second = 0;
3540  }
3541  if( vertexCount.count( key2 )==0 )
3542  {
3543  vertexCount[key2].first = ri2;
3544  vertexCount[key2].second = 0;
3545  }
3546  vertexCount[key1].second++;
3547  vertexCount[key2].second--;
3548  }
3549  else
3550  {
3551  int r1 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+k] );
3552  int r2 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+((k+1)%3)] );
3553  fprintf( stderr , "Bad Edge 2: %d %d\t%d %d\n" , ri1.key , ri2.key , r1 , r2 );
3554  }
3555  }
3556  }
3557  for( int i=0 ; i<int(edges.size()) ; i++ )
3558  {
3559  if( vertexCount.count( edges[i].first.key )==0 ) printf( "Could not find vertex: %lld\n" , edges[i].first );
3560  else if( vertexCount[ edges[i].first.key ].second )
3561  {
3562  RootInfo ri;
3563  GetRootPair( vertexCount[edges[i].first.key].first , fData.depth , ri );
3564  long long key = ri.key;
3565  if( vertexCount.count( key )==0 )
3566  {
3567  int d , off[3];
3568  node->depthAndOffset( d , off );
3569  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] );
3570  }
3571  else
3572  {
3573  edges.push_back( std::pair< RootInfo , RootInfo >( ri , edges[i].first ) );
3574  vertexCount[ key ].second++;
3575  vertexCount[ edges[i].first.key ].second--;
3576  }
3577  }
3578 
3579  if( vertexCount.count( edges[i].second.key )==0 ) printf( "Could not find vertex: %lld\n" , edges[i].second );
3580  else if( vertexCount[edges[i].second.key].second )
3581  {
3582  RootInfo ri;
3583  GetRootPair( vertexCount[edges[i].second.key].first , fData.depth , ri );
3584  long long key = ri.key;
3585  if( vertexCount.count( key ) )
3586  {
3587  int d , off[3];
3588  node->depthAndOffset( d , off );
3589  printf( "Vertex pair not in list 2\t[%d] (%d %d %d)\n" , d , off[0] , off[1] , off[2] );
3590  }
3591  else
3592  {
3593  edges.push_back( std::pair< RootInfo , RootInfo >( edges[i].second , ri ) );
3594  vertexCount[key].second--;
3595  vertexCount[ edges[i].second.key ].second++;
3596  }
3597  }
3598  }
3599  }
3600 
3601  template<int Degree>
3602 #if MISHA_DEBUG
3603  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 )
3604 #else // !MISHA_DEBUG
3605  int Octree< Degree >::GetMCIsoTriangles( TreeOctNode* node , CoredMeshData* mesh , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , int offSet , int sDepth , bool addBarycenter , bool polygonMesh )
3606 #endif // MISHA_DEBUG
3607  {
3608  int tris=0;
3609  std::vector< std::pair< RootInfo , RootInfo > > edges;
3610  std::vector< std::vector< std::pair< RootInfo , RootInfo > > > edgeLoops;
3611  GetMCIsoEdges( node , sDepth , edges );
3612 
3613  GetEdgeLoops( edges , edgeLoops );
3614  for( int i=0 ; i<int(edgeLoops.size()) ; i++ )
3615  {
3616  CoredPointIndex p;
3617  std::vector<CoredPointIndex> edgeIndices;
3618  for( int j=0 ; j<int(edgeLoops[i].size()) ; j++ )
3619  {
3620  if( !GetRootIndex( edgeLoops[i][j].first , rootData , p ) ) printf( "Bad Point Index\n" );
3621  else edgeIndices.push_back( p );
3622  }
3623 #if MISHA_DEBUG
3624  tris += AddTriangles( mesh , edgeIndices , interiorPositions , offSet , polygonMesh , barycenters );
3625 #else // !MISHA_DEBUG
3626  tris += AddTriangles( mesh , edgeIndices , interiorPositions , offSet , addBarycenter , polygonMesh );
3627 #endif // MISHA_DEBUG
3628  }
3629  return tris;
3630  }
3631 
3632  template< int Degree >
3633  int Octree< Degree >::GetEdgeLoops( std::vector< std::pair< RootInfo , RootInfo > >& edges , std::vector< std::vector< std::pair< RootInfo , RootInfo > > >& loops )
3634  {
3635  int loopSize=0;
3636  long long frontIdx , backIdx;
3637  std::pair< RootInfo , RootInfo > e , temp;
3638  loops.clear();
3639 
3640  while( edges.size() )
3641  {
3642  std::vector< std::pair< RootInfo , RootInfo > > front , back;
3643  e = edges[0];
3644  loops.resize( loopSize+1 );
3645  edges[0] = edges.back();
3646  edges.pop_back();
3647  frontIdx = e.second.key;
3648  backIdx = e.first.key;
3649  for( int j=int(edges.size())-1 ; j>=0 ; j-- )
3650  {
3651  if( edges[j].first.key==frontIdx || edges[j].second.key==frontIdx )
3652  {
3653  if( edges[j].first.key==frontIdx ) temp = edges[j];
3654  else temp.first = edges[j].second , temp.second = edges[j].first;
3655  frontIdx = temp.second.key;
3656  front.push_back(temp);
3657  edges[j] = edges.back();
3658  edges.pop_back();
3659  j = int(edges.size());
3660  }
3661  else if( edges[j].first.key==backIdx || edges[j].second.key==backIdx )
3662  {
3663  if( edges[j].second.key==backIdx ) temp = edges[j];
3664  else temp.first = edges[j].second , temp.second = edges[j].first;
3665  backIdx = temp.first.key;
3666  back.push_back(temp);
3667  edges[j] = edges.back();
3668  edges.pop_back();
3669  j = int(edges.size());
3670  }
3671  }
3672  for( int j=int(back.size())-1 ; j>=0 ; j-- ) loops[loopSize].push_back( back[j] );
3673  loops[loopSize].push_back(e);
3674  for( int j=0 ; j<int(front.size()) ; j++ ) loops[loopSize].push_back( front[j] );
3675  loopSize++;
3676  }
3677  return int(loops.size());
3678  }
3679 
3680  template<int Degree>
3681 #if MISHA_DEBUG
3682  int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool polygonMesh , std::vector< Point3D< float > >* barycenters )
3683 #else // !MISHA_DEBUG
3684  int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool addBarycenter , bool polygonMesh )
3685 #endif // MISHA_DEBUG
3686  {
3688  std::vector< Point3D< float > > vertices;
3689  std::vector< TriangleIndex > triangles;
3690  if( polygonMesh )
3691  {
3692  std::vector< CoredVertexIndex > vertices( edges.size() );
3693  for( int i=0 ; i<int(edges.size()) ; i++ )
3694  {
3695  vertices[i].idx = edges[i].index;
3696  vertices[i].inCore = (edges[i].inCore!=0);
3697  }
3698 #pragma omp critical (add_polygon_access)
3699  {
3700  mesh->addPolygon( vertices );
3701  }
3702  return 1;
3703  }
3704  if( edges.size()>3 )
3705  {
3706  bool isCoplanar = false;
3707 
3708 #if MISHA_DEBUG
3709  if( barycenters )
3710 #else // !MISHA_DEBUG
3711  if( addBarycenter )
3712 #endif // MISHA_DEBUG
3713  for( int i=0 ; i<int(edges.size()) ; i++ )
3714  for( int j=0 ; j<i ; j++ )
3715  if( (i+1)%edges.size()!=j && (j+1)%edges.size()!=i )
3716  {
3717  Point3D< Real > v1 , v2;
3718  if( edges[i].inCore ) v1 = mesh->inCorePoints[ edges[i].index ];
3719  else v1 = (*interiorPositions)[ edges[i].index-offSet ];
3720  if( edges[j].inCore ) v2 = mesh->inCorePoints[ edges[j].index ];
3721  else v2 = (*interiorPositions)[ edges[j].index-offSet ];
3722  for( int k=0 ; k<3 ; k++ ) if( v1[k]==v2[k] ) isCoplanar = true;
3723  }
3724  if( isCoplanar )
3725  {
3726  Point3D< Real > c;
3727  c[0] = c[1] = c[2] = 0;
3728  for( int i=0 ; i<int(edges.size()) ; i++ )
3729  {
3730  Point3D<Real> p;
3731  if(edges[i].inCore) p = mesh->inCorePoints[edges[i].index ];
3732  else p = (*interiorPositions)[edges[i].index-offSet];
3733  c += p;
3734  }
3735  c /= Real( edges.size() );
3736  int cIdx;
3737 #pragma omp critical (add_point_access)
3738  {
3739  cIdx = mesh->addOutOfCorePoint( c );
3740 #if MISHA_DEBUG
3741  barycenters->push_back( c );
3742 #else // !MISHA_DEBUG
3743  interiorPositions->push_back( c );
3744 #endif // MISHA_DEBUG
3745  }
3746  for( int i=0 ; i<int(edges.size()) ; i++ )
3747  {
3748  std::vector< CoredVertexIndex > vertices( 3 );
3749  vertices[0].idx = edges[i ].index;
3750  vertices[1].idx = edges[(i+1)%edges.size()].index;
3751  vertices[2].idx = cIdx;
3752  vertices[0].inCore = (edges[i ].inCore!=0);
3753  vertices[1].inCore = (edges[(i+1)%edges.size()].inCore!=0);
3754  vertices[2].inCore = 0;
3755 #pragma omp critical (add_polygon_access)
3756  {
3757  mesh->addPolygon( vertices );
3758  }
3759  }
3760  return int( edges.size() );
3761  }
3762  else
3763  {
3764  vertices.resize( edges.size() );
3765  // Add the points
3766  for( int i=0 ; i<int(edges.size()) ; i++ )
3767  {
3768  Point3D< Real > p;
3769  if( edges[i].inCore ) p = mesh->inCorePoints[edges[i].index ];
3770  else p = (*interiorPositions)[edges[i].index-offSet];
3771  vertices[i] = p;
3772  }
3773  MAT.GetTriangulation( vertices , triangles );
3774  for( int i=0 ; i<int(triangles.size()) ; i++ )
3775  {
3776  std::vector< CoredVertexIndex > _vertices( 3 );
3777  for( int j=0 ; j<3 ; j++ )
3778  {
3779  _vertices[j].idx = edges[ triangles[i].idx[j] ].index;
3780  _vertices[j].inCore = (edges[ triangles[i].idx[j] ].inCore!=0);
3781  }
3782 #pragma omp critical (add_polygon_access)
3783  {
3784  mesh->addPolygon( _vertices );
3785  }
3786  }
3787  }
3788  }
3789  else if( edges.size()==3 )
3790  {
3791  std::vector< CoredVertexIndex > vertices( 3 );
3792  for( int i=0 ; i<3 ; i++ )
3793  {
3794  vertices[i].idx = edges[i].index;
3795  vertices[i].inCore = (edges[i].inCore!=0);
3796  }
3797 #pragma omp critical (add_polygon_access)
3798  mesh->addPolygon( vertices );
3799  }
3800  return int(edges.size())-2;
3801  }
3802 
3803  template< int Degree >
3804  Real* Octree< Degree >::GetSolutionGrid( int& res , float isoValue , int depth )
3805  {
3806  if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth();
3808  fData.set( depth );
3809  fData.setValueTables( fData.VALUE_FLAG );
3810  res = 1<<depth;
3811  Real* values = new float[ res * res * res ];
3812  memset( values , 0 , sizeof( float ) * res * res * res );
3813 
3814  for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode( n ) )
3815  {
3816  if( n->d>depth ) continue;
3817  if( n->d<_minDepth ) continue;
3818  int d , idx[3] , start[3] , end[3];
3819  n->depthAndOffset( d , idx );
3820  for( int i=0 ; i<3 ; i++ )
3821  {
3822  // Get the index of the functions
3823  idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] );
3824  // Figure out which samples fall into the range
3825  fData.setSampleSpan( idx[i] , start[i] , end[i] );
3826  // We only care about the odd indices
3827  if( !(start[i]&1) ) start[i]++;
3828  if( !( end[i]&1) ) end[i]--;
3829  }
3830  Real coefficient = n->nodeData.solution;
3831  for( int x=start[0] ; x<=end[0] ; x+=2 )
3832  for( int y=start[1] ; y<=end[1] ; y+=2 )
3833  for( int z=start[2] ; z<=end[2] ; z+=2 )
3834  {
3835  int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1;
3836  values[ zz*res*res + yy*res + xx ] +=
3837  coefficient *
3838  fData.valueTables[ idx[0]+x*fData.functionCount ] *
3839  fData.valueTables[ idx[1]+y*fData.functionCount ] *
3840  fData.valueTables[ idx[2]+z*fData.functionCount ];
3841  }
3842  }
3843  for( int i=0 ; i<res*res*res ; i++ ) values[i] -= isoValue;
3844 
3845  return values;
3846  }
3847 
3848  template< int Degree >
3849  Real* Octree< Degree >::GetWeightGrid( int& res , int depth )
3850  {
3851  if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth();
3852  res = 1<<tree.maxDepth();
3853  Real* values = new float[ res * res * res ];
3854  memset( values , 0 , sizeof( float ) * res * res * res );
3855 
3856  for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode( n ) )
3857  {
3858  if( n->d>depth ) continue;
3859  int d , idx[3] , start[3] , end[3];
3860  n->depthAndOffset( d , idx );
3861  for( int i=0 ; i<3 ; i++ )
3862  {
3863  // Get the index of the functions
3864  idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] );
3865  // Figure out which samples fall into the range
3866  fData.setSampleSpan( idx[i] , start[i] , end[i] );
3867  // We only care about the odd indices
3868  if( !(start[i]&1) ) start[i]++;
3869  if( !( end[i]&1) ) end[i]--;
3870  }
3871  for( int x=start[0] ; x<=end[0] ; x+=2 )
3872  for( int y=start[1] ; y<=end[1] ; y+=2 )
3873  for( int z=start[2] ; z<=end[2] ; z+=2 )
3874  {
3875  int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1;
3876  values[ zz*res*res + yy*res + xx ] +=
3877  n->nodeData.centerWeightContribution *
3878  fData.valueTables[ idx[0]+x*fData.functionCount ] *
3879  fData.valueTables[ idx[1]+y*fData.functionCount ] *
3880  fData.valueTables[ idx[2]+z*fData.functionCount ];
3881  }
3882  }
3883  return values;
3884  }
3885 
3886  ////////////////
3887  // VertexData //
3888  ////////////////
3889  long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth){
3890  int idx[DIMENSION];
3891  return CenterIndex(node,maxDepth,idx);
3892  }
3893 
3894  long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth,int idx[DIMENSION]){
3895  int d,o[3];
3896  node->depthAndOffset(d,o);
3897  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
3898  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3899  }
3900 
3901  long long VertexData::CenterIndex(int depth,const int offSet[DIMENSION],int maxDepth,int idx[DIMENSION]){
3902  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,depth+1,offSet[i]<<1,1);}
3903  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3904  }
3905 
3906  long long VertexData::CornerIndex(const TreeOctNode* node,int cIndex,int maxDepth){
3907  int idx[DIMENSION];
3908  return CornerIndex(node,cIndex,maxDepth,idx);
3909  }
3910 
3911  long long VertexData::CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int idx[DIMENSION] )
3912  {
3913  int x[DIMENSION];
3914  Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] );
3915  int d , o[3];
3916  node->depthAndOffset( d , o );
3917  for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , d , o[i] , x[i] );
3918  return CornerIndexKey( idx );
3919  }
3920 
3921  long long VertexData::CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int idx[DIMENSION] )
3922  {
3923  int x[DIMENSION];
3924  Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] );
3925  for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , depth , offSet[i] , x[i] );
3926  return CornerIndexKey( idx );
3927  }
3928 
3929  long long VertexData::CornerIndexKey( const int idx[DIMENSION] )
3930  {
3931  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3932  }
3933 
3934  long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth){
3935  int idx[DIMENSION];
3936  return FaceIndex(node,fIndex,maxDepth,idx);
3937  }
3938 
3939  long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth,int idx[DIMENSION]){
3940  int dir,offset;
3941  Cube::FactorFaceIndex(fIndex,dir,offset);
3942  int d,o[3];
3943  node->depthAndOffset(d,o);
3944  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
3945  idx[dir]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,o[dir],offset);
3946  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3947  }
3948 
3949  long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth){
3950  int idx[DIMENSION];
3951  return EdgeIndex(node,eIndex,maxDepth,idx);
3952  }
3953 
3954  long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth,int idx[DIMENSION]){
3955  int o,i1,i2;
3956  int d,off[3];
3957  node->depthAndOffset(d,off);
3958  for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,off[i]<<1,1);}
3959  Cube::FactorEdgeIndex(eIndex,o,i1,i2);
3960  switch(o){
3961  case 0:
3962  idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
3963  idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3964  break;
3965  case 1:
3966  idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3967  idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
3968  break;
3969  case 2:
3970  idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
3971  idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
3972  break;
3973  };
3974  return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
3975  }
3976  }
3977 }
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:396
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:63
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)
std::size_t size() const
Definition: point_cloud.h:434
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)
void setFullDepth(int maxDepth)
void atomicOr(volatile int &dest, int value)
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)
T Norm(std::size_t Ln) const
Definition: vector.hpp:216
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)
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:415
int getMaxEdgeCount(const TreeOctNode *rootNode, int depth, int threads) const
void setCornerTable(CornerTableData &cData, const TreeOctNode *rootNode, int depth, int threads) const
void Resize(std::size_t N)
Definition: vector.hpp:63
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)
void setBSplineData(int maxDepth, Real normalSmooth=-1, bool reflectBoundary=false)
static const int VALUE_FLAG
Definition: bspline_data.h:59