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