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