Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho 00003 All rights reserved. 00004 00005 Redistribution and use in source and binary forms, with or without modification, 00006 are permitted provided that the following conditions are met: 00007 00008 Redistributions of source code must retain the above copyright notice, this list of 00009 conditions and the following disclaimer. Redistributions in binary form must reproduce 00010 the above copyright notice, this list of conditions and the following disclaimer 00011 in the documentation and/or other materials provided with the distribution. 00012 00013 Neither the name of the Johns Hopkins University nor the names of its contributors 00014 may be used to endorse or promote products derived from this software without specific 00015 prior written permission. 00016 00017 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 00018 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES 00019 OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 00020 SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00021 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00022 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR 00023 BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00024 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00025 ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 00026 DAMAGE. 00027 */ 00028 00029 #include "octree_poisson.h" 00030 #include "mat.h" 00031 00032 #if defined WIN32 || defined _WIN32 00033 #if !defined __MINGW32__ 00034 #include <intrin.h> 00035 #include <hash_map> 00036 #endif 00037 #endif 00038 00039 00040 #define ITERATION_POWER 1.0/3 00041 #define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12 00042 #define SPLAT_ORDER 2 00043 00044 #ifndef _MSC_VER 00045 namespace std 00046 { 00047 using namespace __gnu_cxx; 00048 } 00049 #endif 00050 00051 namespace pcl 00052 { 00053 namespace poisson 00054 { 00055 00056 const Real MATRIX_ENTRY_EPSILON = Real(0); 00057 const Real EPSILON=Real(1e-6); 00058 const Real ROUND_EPS=Real(1e-5); 00059 00060 #if defined _WIN32 && !defined __MINGW32__ 00061 using stdext::hash_map; 00062 #else 00063 using std::hash_map; 00064 #endif 00065 00066 00067 void atomicOr(volatile int& dest, int value) 00068 { 00069 #if defined _WIN32 && !defined __MINGW32__ 00070 #if defined (_M_IX86) 00071 _InterlockedOr( (long volatile*)&dest, value ); 00072 #else 00073 InterlockedOr( (long volatile*)&dest , value ); 00074 #endif 00075 #else // !_WIN32 || __MINGW32__ 00076 #pragma omp atomic 00077 dest |= value; 00078 #endif // _WIN32 && !__MINGW32__ 00079 } 00080 00081 00082 ///////////////////// 00083 // SortedTreeNodes // 00084 ///////////////////// 00085 SortedTreeNodes::SortedTreeNodes(void) 00086 { 00087 nodeCount=NULL; 00088 treeNodes=NULL; 00089 maxDepth=0; 00090 } 00091 SortedTreeNodes::~SortedTreeNodes(void){ 00092 if( nodeCount ) delete[] nodeCount; 00093 if( treeNodes ) delete[] treeNodes; 00094 nodeCount = NULL; 00095 treeNodes = NULL; 00096 } 00097 00098 void SortedTreeNodes::set( TreeOctNode& root ) 00099 { 00100 if( nodeCount ) delete[] nodeCount; 00101 if( treeNodes ) delete[] treeNodes; 00102 maxDepth = root.maxDepth()+1; 00103 nodeCount = new int[ maxDepth+1 ]; 00104 treeNodes = new TreeOctNode*[ root.nodes() ]; 00105 00106 nodeCount[0] = 0 , nodeCount[1] = 1; 00107 treeNodes[0] = &root; 00108 for( int d=1 ; d<maxDepth ; d++ ) 00109 { 00110 nodeCount[d+1] = nodeCount[d]; 00111 for( int i=nodeCount[d-1] ; i<nodeCount[d] ; i++ ) 00112 { 00113 TreeOctNode* temp = treeNodes[i]; 00114 if( temp->children ) for( int c=0 ; c<8 ; c++ ) treeNodes[ nodeCount[d+1]++ ] = temp->children + c; 00115 } 00116 } 00117 for( int i=0 ; i<nodeCount[maxDepth] ; i++ ) treeNodes[i]->nodeData.nodeIndex = i; 00118 } 00119 SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::operator[] ( const TreeOctNode* node ) { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } 00120 const SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::operator[] ( const TreeOctNode* node ) const { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } 00121 SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::cornerIndices( const TreeOctNode* node ) { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } 00122 const SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::cornerIndices( const TreeOctNode* node ) const { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } 00123 void SortedTreeNodes::setCornerTable( CornerTableData& cData , const TreeOctNode* rootNode , int maxDepth , int threads ) const 00124 { 00125 if( threads<=0 ) threads = 1; 00126 // The vector of per-depth node spans 00127 std::vector< std::pair< int , int > > spans( this->maxDepth , std::pair< int , int >( -1 , -1 ) ); 00128 int minDepth , off[3]; 00129 rootNode->depthAndOffset( minDepth , off ); 00130 cData.offsets.resize( this->maxDepth , -1 ); 00131 int nodeCount = 0; 00132 { 00133 int start=rootNode->nodeData.nodeIndex , end=start; 00134 for( int d=minDepth ; d<=maxDepth ; d++ ) 00135 { 00136 spans[d] = std::pair< int , int >( start , end+1 ); 00137 cData.offsets[d] = nodeCount - spans[d].first; 00138 nodeCount += spans[d].second - spans[d].first; 00139 if( d<maxDepth ) 00140 { 00141 while( start< end && !treeNodes[start]->children ) start++; 00142 while( end> start && !treeNodes[end ]->children ) end--; 00143 if( start==end && !treeNodes[start]->children ) break; 00144 start = treeNodes[start]->children[0].nodeData.nodeIndex; 00145 end = treeNodes[end ]->children[7].nodeData.nodeIndex; 00146 } 00147 } 00148 } 00149 cData.cTable.resize( nodeCount ); 00150 std::vector< int > count( threads ); 00151 #pragma omp parallel for num_threads( threads ) 00152 for( int t=0 ; t<threads ; t++ ) 00153 { 00154 TreeOctNode::ConstNeighborKey3 neighborKey; 00155 neighborKey.set( maxDepth ); 00156 int offset = nodeCount * t * Cube::CORNERS; 00157 count[t] = 0; 00158 for( int d=minDepth ; d<=maxDepth ; d++ ) 00159 { 00160 int start = spans[d].first , end = spans[d].second , width = end-start; 00161 for( int i=start + (width*t)/threads ; i<start + (width*(t+1))/threads ; i++ ) 00162 { 00163 TreeOctNode* node = treeNodes[i]; 00164 if( d<maxDepth && node->children ) continue; 00165 const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , minDepth ); 00166 for( int c=0 ; c<Cube::CORNERS ; c++ ) // Iterate over the cell's corners 00167 { 00168 bool cornerOwner = true; 00169 int x , y , z; 00170 int ac = Cube::AntipodalCornerIndex( c ); // The index of the node relative to the corner 00171 Cube::FactorCornerIndex( c , x , y , z ); 00172 for( int cc=0 ; cc<Cube::CORNERS ; cc++ ) // Iterate over the corner's cells 00173 { 00174 int xx , yy , zz; 00175 Cube::FactorCornerIndex( cc , xx , yy , zz ); 00176 xx += x , yy += y , zz += z; 00177 if( neighbors.neighbors[xx][yy][zz] ) 00178 if( cc<ac || ( d<maxDepth && neighbors.neighbors[xx][yy][zz]->children ) ) 00179 { 00180 int _d , _off[3]; 00181 neighbors.neighbors[xx][yy][zz]->depthAndOffset( _d , _off ); 00182 _off[0] >>= (d-minDepth) , _off[1] >>= (d-minDepth) , _off[2] >>= (d-minDepth); 00183 if( _off[0]==off[0] && _off[1]==off[1] && _off[2]==off[2] ) 00184 { 00185 cornerOwner = false; 00186 break; 00187 } 00188 else fprintf( stderr , "[WARNING] How did we leave the subtree?\n" ); 00189 } 00190 } 00191 if( cornerOwner ) 00192 { 00193 const TreeOctNode* n = node; 00194 int d = n->depth(); 00195 do 00196 { 00197 const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.neighbors[d]; 00198 // Set all the corner indices at the current depth 00199 for( int cc=0 ; cc<Cube::CORNERS ; cc++ ) 00200 { 00201 int xx , yy , zz; 00202 Cube::FactorCornerIndex( cc , xx , yy , zz ); 00203 xx += x , yy += y , zz += z; 00204 if( neighborKey.neighbors[d].neighbors[xx][yy][zz] ) 00205 cData[ neighbors.neighbors[xx][yy][zz] ][ Cube::AntipodalCornerIndex(cc) ] = count[t] + offset; 00206 } 00207 // If we are not at the root and the parent also has the corner 00208 if( d==minDepth || n!=(n->parent->children+c) ) break; 00209 n = n->parent; 00210 d--; 00211 } 00212 while( 1 ); 00213 count[t]++; 00214 } 00215 } 00216 } 00217 } 00218 } 00219 cData.cCount = 0; 00220 std::vector< int > offsets( threads+1 ); 00221 offsets[0] = 0; 00222 for( int t=0 ; t<threads ; t++ ) cData.cCount += count[t] , offsets[t+1] = offsets[t] + count[t]; 00223 #pragma omp parallel for num_threads( threads ) 00224 for( int t=0 ; t<threads ; t++ ) 00225 for( int d=minDepth ; d<=maxDepth ; d++ ) 00226 { 00227 int start = spans[d].first , end = spans[d].second , width = end - start; 00228 for( int i=start + (width*t)/threads ; i<start+(width*(t+1))/threads ; i++ ) 00229 for( int c=0 ; c<Cube::CORNERS ; c++ ) 00230 { 00231 int& idx = cData[ treeNodes[i] ][c]; 00232 if( idx<0 ) 00233 { 00234 fprintf( stderr , "[ERROR] Found unindexed corner nodes[%d][%d] = %d (%d,%d)\n" , treeNodes[i]->nodeData.nodeIndex , c , idx , minDepth , maxDepth ); 00235 int _d , _off[3]; 00236 treeNodes[i]->depthAndOffset( _d , _off ); 00237 printf( "(%d [%d %d %d) <-> (%d [%d %d %d])\n" , minDepth , off[0] , off[1] , off[2] , _d , _off[0] , _off[1] , _off[2] ); 00238 printf( "[%d %d]\n" , spans[d].first , spans[d].second ); 00239 exit( 0 ); 00240 } 00241 else 00242 { 00243 int div = idx / ( nodeCount*Cube::CORNERS ); 00244 int rem = idx % ( nodeCount*Cube::CORNERS ); 00245 idx = rem + offsets[div]; 00246 } 00247 } 00248 } 00249 } 00250 int SortedTreeNodes::getMaxCornerCount( const TreeOctNode* rootNode , int depth , int maxDepth , int threads ) const 00251 { 00252 if( threads<=0 ) threads = 1; 00253 int res = 1<<depth; 00254 std::vector< std::vector< int > > cornerCount( threads ); 00255 for( int t=0 ; t<threads ; t++ ) cornerCount[t].resize( res*res*res , 0 ); 00256 00257 #pragma omp parallel for num_threads( threads ) 00258 for( int t=0 ; t<threads ; t++ ) 00259 { 00260 std::vector< int >& _cornerCount = cornerCount[t]; 00261 TreeOctNode::ConstNeighborKey3 neighborKey; 00262 neighborKey.set( maxDepth ); 00263 int start = nodeCount[depth] , end = nodeCount[maxDepth+1] , range = end-start; 00264 for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ ) 00265 { 00266 TreeOctNode* node = treeNodes[start+i]; 00267 int d , off[3]; 00268 node->depthAndOffset( d , off ); 00269 if( d<maxDepth && node->children ) continue; 00270 00271 const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , depth ); 00272 for( int c=0 ; c<Cube::CORNERS ; c++ ) // Iterate over the cell's corners 00273 { 00274 bool cornerOwner = true; 00275 int x , y , z; 00276 int ac = Cube::AntipodalCornerIndex( c ); // The index of the node relative to the corner 00277 Cube::FactorCornerIndex( c , x , y , z ); 00278 for( int cc=0 ; cc<Cube::CORNERS ; cc++ ) // Iterate over the corner's cells 00279 { 00280 int xx , yy , zz; 00281 Cube::FactorCornerIndex( cc , xx , yy , zz ); 00282 xx += x , yy += y , zz += z; 00283 if( neighbors.neighbors[xx][yy][zz] ) 00284 if( cc<ac || ( d<maxDepth && neighbors.neighbors[xx][yy][zz]->children ) ) 00285 { 00286 cornerOwner = false; 00287 break; 00288 } 00289 } 00290 if( cornerOwner ) _cornerCount[ ( ( off[0]>>(d-depth) ) * res * res) + ( ( off[1]>>(d-depth) ) * res) + ( off[2]>>(d-depth) ) ]++; 00291 } 00292 } 00293 } 00294 int maxCount = 0; 00295 for( int i=0 ; i<res*res*res ; i++ ) 00296 { 00297 int c = 0; 00298 for( int t=0 ; t<threads ; t++ ) c += cornerCount[t][i]; 00299 maxCount = std::max< int >( maxCount , c ); 00300 } 00301 return maxCount; 00302 } 00303 SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::operator[] ( const TreeOctNode* node ) { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } 00304 const SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::operator[] ( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } 00305 SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::edgeIndices( const TreeOctNode* node ) { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } 00306 const SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::edgeIndices( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; } 00307 void SortedTreeNodes::setEdgeTable( EdgeTableData& eData , const TreeOctNode* rootNode , int maxDepth , int threads ) 00308 { 00309 if( threads<=0 ) threads = 1; 00310 std::vector< std::pair< int , int > > spans( this->maxDepth , std::pair< int , int >( -1 , -1 ) ); 00311 00312 int minDepth , off[3]; 00313 rootNode->depthAndOffset( minDepth , off ); 00314 eData.offsets.resize( this->maxDepth , -1 ); 00315 int nodeCount = 0; 00316 { 00317 int start=rootNode->nodeData.nodeIndex , end=start; 00318 for( int d=minDepth ; d<=maxDepth ; d++ ) 00319 { 00320 spans[d] = std::pair< int , int >( start , end+1 ); 00321 eData.offsets[d] = nodeCount - spans[d].first; 00322 nodeCount += spans[d].second - spans[d].first; 00323 if( d<maxDepth ) 00324 { 00325 while( start< end && !treeNodes[start]->children ) start++; 00326 while( end> start && !treeNodes[end ]->children ) end--; 00327 if( start==end && !treeNodes[start]->children ) break; 00328 start = treeNodes[start]->children[0].nodeData.nodeIndex; 00329 end = treeNodes[end ]->children[7].nodeData.nodeIndex; 00330 } 00331 } 00332 } 00333 eData.eTable.resize( nodeCount ); 00334 std::vector< int > count( threads ); 00335 #pragma omp parallel for num_threads( threads ) 00336 for( int t=0 ; t<threads ; t++ ) 00337 { 00338 TreeOctNode::ConstNeighborKey3 neighborKey; 00339 neighborKey.set( maxDepth ); 00340 int offset = nodeCount * t * Cube::EDGES; 00341 count[t] = 0; 00342 for( int d=minDepth ; d<=maxDepth ; d++ ) 00343 { 00344 int start = spans[d].first , end = spans[d].second , width = end-start; 00345 for( int i=start + (width*t)/threads ; i<start + (width*(t+1))/threads ; i++ ) 00346 { 00347 TreeOctNode* node = treeNodes[i]; 00348 const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , minDepth ); 00349 00350 for( int e=0 ; e<Cube::EDGES ; e++ ) 00351 { 00352 bool edgeOwner = true; 00353 int o , i , j; 00354 Cube::FactorEdgeIndex( e , o , i , j ); 00355 int ac = Square::AntipodalCornerIndex( Square::CornerIndex( i , j ) ); 00356 for( int cc=0 ; cc<Square::CORNERS ; cc++ ) 00357 { 00358 int ii , jj , x , y , z; 00359 Square::FactorCornerIndex( cc , ii , jj ); 00360 ii += i , jj += j; 00361 switch( o ) 00362 { 00363 case 0: y = ii , z = jj , x = 1 ; break; 00364 case 1: x = ii , z = jj , y = 1 ; break; 00365 case 2: x = ii , y = jj , z = 1 ; break; 00366 } 00367 if( neighbors.neighbors[x][y][z] && cc<ac ) { edgeOwner = false ; break; } 00368 } 00369 if( edgeOwner ) 00370 { 00371 // Set all edge indices 00372 for( int cc=0 ; cc<Square::CORNERS ; cc++ ) 00373 { 00374 int ii , jj , aii , ajj , x , y , z; 00375 Square::FactorCornerIndex( cc , ii , jj ); 00376 Square::FactorCornerIndex( Square::AntipodalCornerIndex( cc ) , aii , ajj ); 00377 ii += i , jj += j; 00378 switch( o ) 00379 { 00380 case 0: y = ii , z = jj , x = 1 ; break; 00381 case 1: x = ii , z = jj , y = 1 ; break; 00382 case 2: x = ii , y = jj , z = 1 ; break; 00383 } 00384 if( neighbors.neighbors[x][y][z] ) 00385 eData[ neighbors.neighbors[x][y][z] ][ Cube::EdgeIndex( o , aii , ajj ) ] = count[t]+offset; 00386 } 00387 count[t]++; 00388 } 00389 } 00390 } 00391 } 00392 } 00393 eData.eCount = 0; 00394 std::vector< int > offsets( threads+1 ); 00395 offsets[0] = 0; 00396 for( int t=0 ; t<threads ; t++ ) eData.eCount += count[t] , offsets[t+1] = offsets[t] + count[t]; 00397 #pragma omp parallel for num_threads( threads ) 00398 for( int t=0 ; t<threads ; t++ ) 00399 for( int d=minDepth ; d<=maxDepth ; d++ ) 00400 { 00401 int start = spans[d].first , end = spans[d].second , width = end - start; 00402 for( int i=start + (width*t)/threads ; i<start+(width*(t+1))/threads ; i++ ) 00403 for( int e=0 ; e<Cube::EDGES ; e++ ) 00404 { 00405 int& idx = eData[ treeNodes[i] ][e]; 00406 if( idx<0 ) fprintf( stderr , "[ERROR] Found unindexed edge %d (%d,%d)\n" , idx , minDepth , maxDepth ) , exit( 0 ); 00407 else 00408 { 00409 int div = idx / ( nodeCount*Cube::EDGES ); 00410 int rem = idx % ( nodeCount*Cube::EDGES ); 00411 idx = rem + offsets[div]; 00412 } 00413 } 00414 } 00415 } 00416 int SortedTreeNodes::getMaxEdgeCount( const TreeOctNode* rootNode , int depth , int threads ) const 00417 { 00418 if( threads<=0 ) threads = 1; 00419 int res = 1<<depth; 00420 std::vector< std::vector< int > > edgeCount( threads ); 00421 for( int t=0 ; t<threads ; t++ ) edgeCount[t].resize( res*res*res , 0 ); 00422 00423 #pragma omp parallel for num_threads( threads ) 00424 for( int t=0 ; t<threads ; t++ ) 00425 { 00426 std::vector< int >& _edgeCount = edgeCount[t]; 00427 TreeOctNode::ConstNeighborKey3 neighborKey; 00428 neighborKey.set( maxDepth-1 ); 00429 int start = nodeCount[depth] , end = nodeCount[maxDepth] , range = end-start; 00430 for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ ) 00431 { 00432 TreeOctNode* node = treeNodes[start+i]; 00433 const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , depth ); 00434 int d , off[3]; 00435 node->depthAndOffset( d , off ); 00436 00437 for( int e=0 ; e<Cube::EDGES ; e++ ) 00438 { 00439 bool edgeOwner = true; 00440 int o , i , j; 00441 Cube::FactorEdgeIndex( e , o , i , j ); 00442 int ac = Square::AntipodalCornerIndex( Square::CornerIndex( i , j ) ); 00443 for( int cc=0 ; cc<Square::CORNERS ; cc++ ) 00444 { 00445 int ii , jj , x , y , z; 00446 Square::FactorCornerIndex( cc , ii , jj ); 00447 ii += i , jj += j; 00448 switch( o ) 00449 { 00450 case 0: y = ii , z = jj , x = 1 ; break; 00451 case 1: x = ii , z = jj , y = 1 ; break; 00452 case 2: x = ii , y = jj , z = 1 ; break; 00453 } 00454 if( neighbors.neighbors[x][y][z] && cc<ac ) { edgeOwner = false ; break; } 00455 } 00456 if( edgeOwner ) _edgeCount[ ( ( off[0]>>(d-depth) ) * res * res) + ( ( off[1]>>(d-depth) ) * res) + ( off[2]>>(d-depth) ) ]++; 00457 } 00458 } 00459 } 00460 int maxCount = 0; 00461 for( int i=0 ; i<res*res*res ; i++ ) 00462 { 00463 int c = 0; 00464 for( int t=0 ; t<threads ; t++ ) c += edgeCount[t][i]; 00465 maxCount = std::max< int >( maxCount , c ); 00466 } 00467 return maxCount; 00468 } 00469 00470 00471 00472 ////////////////// 00473 // TreeNodeData // 00474 ////////////////// 00475 int TreeNodeData::UseIndex=1; 00476 TreeNodeData::TreeNodeData( void ) 00477 { 00478 if( UseIndex ) 00479 { 00480 nodeIndex = -1; 00481 centerWeightContribution=0; 00482 } 00483 else mcIndex=0; 00484 normalIndex = -1; 00485 constraint = solution = 0; 00486 pointIndex = -1; 00487 } 00488 TreeNodeData::~TreeNodeData( void ) { } 00489 00490 00491 //////////// 00492 // Octree // 00493 //////////// 00494 template<int Degree> 00495 double Octree<Degree>::maxMemoryUsage=0; 00496 00497 00498 00499 template<int Degree> 00500 double Octree<Degree>::MemoryUsage(void){ 00501 double mem = 0;//double( MemoryInfo::Usage() ) / (1<<20); 00502 if(mem>maxMemoryUsage){maxMemoryUsage=mem;} 00503 return mem; 00504 } 00505 00506 template<int Degree> 00507 Octree<Degree>::Octree(void) 00508 { 00509 threads = 1; 00510 radius = 0; 00511 width = 0; 00512 postNormalSmooth = 0; 00513 _constrainValues = false; 00514 } 00515 00516 template<int Degree> 00517 int Octree<Degree>::NonLinearSplatOrientedPoint( TreeOctNode* node , const Point3D<Real>& position , const Point3D<Real>& normal ) 00518 { 00519 double x , dxdy , dxdydz , dx[DIMENSION][SPLAT_ORDER+1]; 00520 int off[3]; 00521 TreeOctNode::Neighbors3& neighbors = neighborKey.setNeighbors( node ); 00522 double width; 00523 Point3D<Real> center; 00524 Real w; 00525 node->centerAndWidth( center , w ); 00526 width=w; 00527 for( int i=0 ; i<3 ; i++ ) 00528 { 00529 #if SPLAT_ORDER==2 00530 off[i] = 0; 00531 x = ( center[i] - position[i] - width ) / width; 00532 dx[i][0] = 1.125+1.500*x+0.500*x*x; 00533 x = ( center[i] - position[i] ) / width; 00534 dx[i][1] = 0.750 - x*x; 00535 00536 dx[i][2] = 1. - dx[i][1] - dx[i][0]; 00537 #elif SPLAT_ORDER==1 00538 x = ( position[i] - center[i] ) / width; 00539 if( x<0 ) 00540 { 00541 off[i] = 0; 00542 dx[i][0] = -x; 00543 } 00544 else 00545 { 00546 off[i] = 1; 00547 dx[i][0] = 1. - x; 00548 } 00549 dx[i][1] = 1. - dx[i][0]; 00550 #elif SPLAT_ORDER==0 00551 off[i] = 1; 00552 dx[i][0] = 1.; 00553 #else 00554 # error Splat order not supported 00555 #endif // SPLAT_ORDER 00556 } 00557 for( int i=off[0] ; i<=off[0]+SPLAT_ORDER ; i++ ) for( int j=off[1] ; j<=off[1]+SPLAT_ORDER ; j++ ) 00558 { 00559 dxdy = dx[0][i] * dx[1][j]; 00560 for( int k=off[2] ; k<=off[2]+SPLAT_ORDER ; k++ ) 00561 if( neighbors.neighbors[i][j][k] ) 00562 { 00563 dxdydz = dxdy * dx[2][k]; 00564 TreeOctNode* _node = neighbors.neighbors[i][j][k]; 00565 int idx =_node->nodeData.normalIndex; 00566 if( idx<0 ) 00567 { 00568 Point3D<Real> n; 00569 n[0] = n[1] = n[2] = 0; 00570 _node->nodeData.nodeIndex = 0; 00571 idx = _node->nodeData.normalIndex = int(normals->size()); 00572 normals->push_back(n); 00573 } 00574 (*normals)[idx] += normal * Real( dxdydz ); 00575 } 00576 } 00577 return 0; 00578 } 00579 template<int Degree> 00580 Real Octree<Degree>::NonLinearSplatOrientedPoint( const Point3D<Real>& position , const Point3D<Real>& normal , int splatDepth , Real samplesPerNode , 00581 int minDepth , int maxDepth ) 00582 { 00583 double dx; 00584 Point3D<Real> n; 00585 TreeOctNode* temp; 00586 int cnt=0; 00587 double width; 00588 Point3D< Real > myCenter; 00589 Real myWidth; 00590 myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5); 00591 myWidth = Real(1.0); 00592 00593 temp = &tree; 00594 while( temp->depth()<splatDepth ) 00595 { 00596 if( !temp->children ) 00597 { 00598 fprintf( stderr , "Octree<Degree>::NonLinearSplatOrientedPoint error\n" ); 00599 return -1; 00600 } 00601 int cIndex=TreeOctNode::CornerIndex(myCenter,position); 00602 temp=&temp->children[cIndex]; 00603 myWidth/=2; 00604 if(cIndex&1) myCenter[0] += myWidth/2; 00605 else myCenter[0] -= myWidth/2; 00606 if(cIndex&2) myCenter[1] += myWidth/2; 00607 else myCenter[1] -= myWidth/2; 00608 if(cIndex&4) myCenter[2] += myWidth/2; 00609 else myCenter[2] -= myWidth/2; 00610 } 00611 Real alpha,newDepth; 00612 NonLinearGetSampleDepthAndWeight( temp , position , samplesPerNode , newDepth , alpha ); 00613 00614 if( newDepth<minDepth ) newDepth=Real(minDepth); 00615 if( newDepth>maxDepth ) newDepth=Real(maxDepth); 00616 int topDepth=int(ceil(newDepth)); 00617 00618 dx = 1.0-(topDepth-newDepth); 00619 if( topDepth<=minDepth ) 00620 { 00621 topDepth=minDepth; 00622 dx=1; 00623 } 00624 else if( topDepth>maxDepth ) 00625 { 00626 topDepth=maxDepth; 00627 dx=1; 00628 } 00629 while( temp->depth()>topDepth ) temp=temp->parent; 00630 while( temp->depth()<topDepth ) 00631 { 00632 if(!temp->children) temp->initChildren(); 00633 int cIndex=TreeOctNode::CornerIndex(myCenter,position); 00634 temp=&temp->children[cIndex]; 00635 myWidth/=2; 00636 if(cIndex&1) myCenter[0] += myWidth/2; 00637 else myCenter[0] -= myWidth/2; 00638 if(cIndex&2) myCenter[1] += myWidth/2; 00639 else myCenter[1] -= myWidth/2; 00640 if(cIndex&4) myCenter[2] += myWidth/2; 00641 else myCenter[2] -= myWidth/2; 00642 } 00643 width = 1.0 / ( 1<<temp->depth() ); 00644 n = normal * alpha / Real( pow( width , 3 ) ) * Real( dx ); 00645 NonLinearSplatOrientedPoint( temp , position , n ); 00646 if( fabs(1.0-dx) > EPSILON ) 00647 { 00648 dx = Real(1.0-dx); 00649 temp = temp->parent; 00650 width = 1.0 / ( 1<<temp->depth() ); 00651 00652 n = normal * alpha / Real( pow( width , 3 ) ) * Real( dx ); 00653 NonLinearSplatOrientedPoint( temp , position , n ); 00654 } 00655 return alpha; 00656 } 00657 template<int Degree> 00658 void Octree<Degree>::NonLinearGetSampleDepthAndWeight(TreeOctNode* node,const Point3D<Real>& position,Real samplesPerNode,Real& depth,Real& weight){ 00659 TreeOctNode* temp=node; 00660 weight = Real(1.0)/NonLinearGetSampleWeight(temp,position); 00661 if( weight>=samplesPerNode ) depth=Real( temp->depth() + log( weight / samplesPerNode ) / log(double(1<<(DIMENSION-1))) ); 00662 else 00663 { 00664 Real oldAlpha,newAlpha; 00665 oldAlpha=newAlpha=weight; 00666 while( newAlpha<samplesPerNode && temp->parent ) 00667 { 00668 temp=temp->parent; 00669 oldAlpha=newAlpha; 00670 newAlpha=Real(1.0)/NonLinearGetSampleWeight(temp,position); 00671 } 00672 depth = Real( temp->depth() + log( newAlpha / samplesPerNode ) / log( newAlpha / oldAlpha ) ); 00673 } 00674 weight=Real(pow(double(1<<(DIMENSION-1)),-double(depth))); 00675 } 00676 00677 template<int Degree> 00678 Real Octree<Degree>::NonLinearGetSampleWeight( TreeOctNode* node , const Point3D<Real>& position ) 00679 { 00680 Real weight=0; 00681 double x,dxdy,dx[DIMENSION][3]; 00682 TreeOctNode::Neighbors3& neighbors=neighborKey.setNeighbors( node ); 00683 double width; 00684 Point3D<Real> center; 00685 Real w; 00686 node->centerAndWidth(center,w); 00687 width=w; 00688 00689 for( int i=0 ; i<DIMENSION ; i++ ) 00690 { 00691 x = ( center[i] - position[i] - width ) / width; 00692 dx[i][0] = 1.125 + 1.500*x + 0.500*x*x; 00693 x = ( center[i] - position[i] ) / width; 00694 dx[i][1] = 0.750 - x*x; 00695 00696 dx[i][2] = 1.0 - dx[i][1] - dx[i][0]; 00697 } 00698 00699 for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) 00700 { 00701 dxdy = dx[0][i] * dx[1][j]; 00702 for( int k=0 ; k<3 ; k++ ) if( neighbors.neighbors[i][j][k] ) 00703 weight += Real( dxdy * dx[2][k] * neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution ); 00704 } 00705 return Real( 1.0 / weight ); 00706 } 00707 00708 template<int Degree> 00709 int Octree<Degree>::NonLinearUpdateWeightContribution( TreeOctNode* node , const Point3D<Real>& position , Real weight ) 00710 { 00711 TreeOctNode::Neighbors3& neighbors = neighborKey.setNeighbors( node ); 00712 double x,dxdy,dx[DIMENSION][3]; 00713 double width; 00714 Point3D<Real> center; 00715 Real w; 00716 node->centerAndWidth( center , w ); 00717 width=w; 00718 const double SAMPLE_SCALE = 1. / ( 0.125 * 0.125 + 0.75 * 0.75 + 0.125 * 0.125 ); 00719 00720 for( int i=0 ; i<DIMENSION ; i++ ) 00721 { 00722 x = ( center[i] - position[i] - width ) / width; 00723 dx[i][0] = 1.125 + 1.500*x + 0.500*x*x; 00724 x = ( center[i] - position[i] ) / width; 00725 dx[i][1] = 0.750 - x*x; 00726 dx[i][2] = 1. - dx[i][1] - dx[i][0]; 00727 // Note that we are splatting along a co-dimension one manifold, so uniform point samples 00728 // do not generate a unit sample weight. 00729 dx[i][0] *= SAMPLE_SCALE; 00730 } 00731 00732 for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) 00733 { 00734 dxdy = dx[0][i] * dx[1][j] * weight; 00735 for( int k=0 ; k<3 ; k++ ) if( neighbors.neighbors[i][j][k] ) 00736 neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution += Real( dxdy * dx[2][k] ); 00737 } 00738 return 0; 00739 } 00740 00741 template< int Degree > template<typename PointNT> int 00742 Octree<Degree>::setTree( boost::shared_ptr<const pcl::PointCloud<PointNT> > input_, int maxDepth , int minDepth , 00743 int kernelDepth , Real samplesPerNode , Real scaleFactor , Point3D<Real>& center , Real& scale , 00744 int useConfidence , Real constraintWeight , bool adaptiveWeights ) 00745 { 00746 _minDepth = std::min< int >( std::max< int >( 0 , minDepth ) , maxDepth ); 00747 _constrainValues = (constraintWeight>0); 00748 double pointWeightSum = 0; 00749 Point3D<Real> min , max , position , normal , myCenter; 00750 Real myWidth; 00751 int i , cnt=0; 00752 TreeOctNode* temp; 00753 int splatDepth=0; 00754 00755 TreeNodeData::UseIndex = 1; 00756 neighborKey.set( maxDepth ); 00757 splatDepth = kernelDepth; 00758 if( splatDepth<0 ) splatDepth = 0; 00759 00760 00761 tree.setFullDepth( _minDepth ); 00762 // Read through once to get the center and scale 00763 while (cnt != input_->size ()) 00764 { 00765 Point3D< Real > p; 00766 p[0] = input_->points[cnt].x; 00767 p[1] = input_->points[cnt].y; 00768 p[2] = input_->points[cnt].z; 00769 00770 for (i = 0; i < DIMENSION; i++) 00771 { 00772 if( !cnt || p[i]<min[i] ) min[i] = p[i]; 00773 if( !cnt || p[i]>max[i] ) max[i] = p[i]; 00774 } 00775 cnt++; 00776 } 00777 00778 scale = std::max< Real >( max[0]-min[0] , std::max< Real >( max[1]-min[1] , max[2]-min[2] ) ); 00779 center = ( max+min ) /2; 00780 00781 scale *= scaleFactor; 00782 for( i=0 ; i<DIMENSION ; i++ ) center[i] -= scale/2; 00783 if( splatDepth>0 ) 00784 { 00785 cnt = 0; 00786 while (cnt != input_->size ()) 00787 { 00788 position[0] = input_->points[cnt].x; 00789 position[1] = input_->points[cnt].y; 00790 position[2] = input_->points[cnt].z; 00791 normal[0] = input_->points[cnt].normal_x; 00792 normal[1] = input_->points[cnt].normal_y; 00793 normal[2] = input_->points[cnt].normal_z; 00794 00795 for( i=0 ; i<DIMENSION ; i++ ) position[i] = ( position[i]-center[i] ) / scale; 00796 myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5); 00797 myWidth = Real(1.0); 00798 for( i=0 ; i<DIMENSION ; i++ ) if( position[i]<myCenter[i]-myWidth/2 || position[i]>myCenter[i]+myWidth/2 ) break; 00799 if( i!=DIMENSION ) continue; 00800 Real weight=Real( 1. ); 00801 if( useConfidence ) weight = Real( Length(normal) ); 00802 temp = &tree; 00803 int d=0; 00804 while( d<splatDepth ) 00805 { 00806 NonLinearUpdateWeightContribution( temp , position , weight ); 00807 if( !temp->children ) temp->initChildren(); 00808 int cIndex=TreeOctNode::CornerIndex(myCenter,position); 00809 temp=&temp->children[cIndex]; 00810 myWidth/=2; 00811 if(cIndex&1) myCenter[0] += myWidth/2; 00812 else myCenter[0] -= myWidth/2; 00813 if(cIndex&2) myCenter[1] += myWidth/2; 00814 else myCenter[1] -= myWidth/2; 00815 if(cIndex&4) myCenter[2] += myWidth/2; 00816 else myCenter[2] -= myWidth/2; 00817 d++; 00818 } 00819 NonLinearUpdateWeightContribution( temp , position , weight ); 00820 cnt++; 00821 } 00822 } 00823 00824 normals = new std::vector< Point3D<Real> >(); 00825 cnt=0; 00826 while (cnt != input_->size ()) 00827 { 00828 position[0] = input_->points[cnt].x; 00829 position[1] = input_->points[cnt].y; 00830 position[2] = input_->points[cnt].z; 00831 normal[0] = input_->points[cnt].normal_x; 00832 normal[1] = input_->points[cnt].normal_y; 00833 normal[2] = input_->points[cnt].normal_z; 00834 cnt ++; 00835 for( i=0 ; i<DIMENSION ; i++ ) position[i] = ( position[i]-center[i] ) / scale; 00836 myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5); 00837 myWidth = Real(1.0); 00838 for( i=0 ; i<DIMENSION ; i++ ) if(position[i]<myCenter[i]-myWidth/2 || position[i]>myCenter[i]+myWidth/2) break; 00839 if( i!=DIMENSION ) continue; 00840 Real l = Real( Length( normal ) ); 00841 if( l!=l || l<=EPSILON ) continue; 00842 if( !useConfidence ) normal /= l; 00843 00844 l = Real(1.); 00845 Real pointWeight = Real(1.f); 00846 if( samplesPerNode>0 && splatDepth ) 00847 { 00848 pointWeight = NonLinearSplatOrientedPoint( position , normal , splatDepth , samplesPerNode , _minDepth , maxDepth ); 00849 } 00850 else 00851 { 00852 Real alpha=1; 00853 temp = &tree; 00854 int d=0; 00855 if( splatDepth ) 00856 { 00857 while( d<splatDepth ) 00858 { 00859 int cIndex=TreeOctNode::CornerIndex(myCenter,position); 00860 temp=&temp->children[cIndex]; 00861 myWidth/=2; 00862 if(cIndex&1) myCenter[0]+=myWidth/2; 00863 else myCenter[0]-=myWidth/2; 00864 if(cIndex&2) myCenter[1]+=myWidth/2; 00865 else myCenter[1]-=myWidth/2; 00866 if(cIndex&4) myCenter[2]+=myWidth/2; 00867 else myCenter[2]-=myWidth/2; 00868 d++; 00869 } 00870 alpha = NonLinearGetSampleWeight( temp , position ); 00871 } 00872 for( i=0 ; i<DIMENSION ; i++ ) normal[i]*=alpha; 00873 while( d<maxDepth ) 00874 { 00875 if(!temp->children){temp->initChildren();} 00876 int cIndex=TreeOctNode::CornerIndex(myCenter,position); 00877 temp=&temp->children[cIndex]; 00878 myWidth/=2; 00879 if(cIndex&1) myCenter[0]+=myWidth/2; 00880 else myCenter[0]-=myWidth/2; 00881 if(cIndex&2) myCenter[1]+=myWidth/2; 00882 else myCenter[1]-=myWidth/2; 00883 if(cIndex&4) myCenter[2]+=myWidth/2; 00884 else myCenter[2]-=myWidth/2; 00885 d++; 00886 } 00887 NonLinearSplatOrientedPoint( temp , position , normal ); 00888 pointWeight = alpha; 00889 } 00890 pointWeight = 1; 00891 pointWeightSum += pointWeight; 00892 if( _constrainValues ) 00893 { 00894 int d = 0; 00895 TreeOctNode* temp = &tree; 00896 myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5); 00897 myWidth = Real(1.0); 00898 while( 1 ) 00899 { 00900 int idx = temp->nodeData.pointIndex; 00901 if( idx==-1 ) 00902 { 00903 Point3D< Real > p; 00904 p[0] = p[1] = p[2] = 0; 00905 idx = int( _points.size() ); 00906 _points.push_back( PointData( position*pointWeight , pointWeight ) ); 00907 temp->nodeData.pointIndex = idx; 00908 } 00909 else 00910 { 00911 _points[idx].weight += pointWeight; 00912 _points[idx].position += position * pointWeight; 00913 } 00914 00915 int cIndex = TreeOctNode::CornerIndex( myCenter , position ); 00916 if( !temp->children ) break; 00917 temp = &temp->children[cIndex]; 00918 myWidth /= 2; 00919 if( cIndex&1 ) myCenter[0] += myWidth/2; 00920 else myCenter[0] -= myWidth/2; 00921 if( cIndex&2 ) myCenter[1] += myWidth/2; 00922 else myCenter[1] -= myWidth/2; 00923 if( cIndex&4 ) myCenter[2] += myWidth/2; 00924 else myCenter[2] -= myWidth/2; 00925 d++; 00926 } 00927 } 00928 } 00929 00930 00931 if( _constrainValues ) 00932 for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode(n) ) 00933 if( n->nodeData.pointIndex!=-1 ) 00934 { 00935 int idx = n->nodeData.pointIndex; 00936 _points[idx].position /= _points[idx].weight; 00937 if( adaptiveWeights ) _points[idx].weight *= (1<<n->d); 00938 else _points[idx].weight *= (1<<maxDepth); 00939 _points[idx].weight *= Real( constraintWeight / pointWeightSum ); 00940 } 00941 #if FORCE_NEUMANN_FIELD 00942 for( TreeOctNode* node=tree.nextNode() ; node ; node=tree.nextNode( node ) ) 00943 { 00944 int d , off[3] , res; 00945 node->depthAndOffset( d , off ); 00946 res = 1<<d; 00947 if( node->nodeData.normalIndex<0 ) continue; 00948 Point3D< Real >& normal = (*normals)[node->nodeData.normalIndex]; 00949 for( int d=0 ; d<3 ; d++ ) if( off[d]==0 || off[d]==res-1 ) normal[d] = 0; 00950 } 00951 #endif // FORCE_NEUMANN_FIELD 00952 _sNodes.set( tree ); 00953 00954 00955 return cnt; 00956 } 00957 00958 00959 template<int Degree> 00960 void Octree<Degree>::setBSplineData( int maxDepth , Real normalSmooth , bool reflectBoundary ) 00961 { 00962 radius = 0.5 + 0.5 * Degree; 00963 width=int(double(radius+0.5-EPSILON)*2); 00964 if( normalSmooth>0 ) postNormalSmooth = normalSmooth; 00965 fData.set( maxDepth , true , reflectBoundary ); 00966 } 00967 00968 template<int Degree> 00969 void Octree<Degree>::finalize( void ) 00970 { 00971 int maxDepth = tree.maxDepth( ); 00972 TreeOctNode::NeighborKey5 nKey; 00973 nKey.set( maxDepth ); 00974 for( int d=maxDepth ; d>0 ; d-- ) 00975 for( TreeOctNode* node=tree.nextNode() ; node ; node=tree.nextNode( node ) ) 00976 if( node->d==d ) 00977 { 00978 int xStart=0 , xEnd=5 , yStart=0 , yEnd=5 , zStart=0 , zEnd=5; 00979 int c = int( node - node->parent->children ); 00980 int x , y , z; 00981 Cube::FactorCornerIndex( c , x , y , z ); 00982 if( x ) xStart = 1; 00983 else xEnd = 4; 00984 if( y ) yStart = 1; 00985 else yEnd = 4; 00986 if( z ) zStart = 1; 00987 else zEnd = 4; 00988 nKey.setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd ); 00989 } 00990 _sNodes.set( tree ); 00991 MemoryUsage(); 00992 } 00993 template< int Degree > 00994 Real Octree< Degree >::GetValue( const PointInfo points[3][3][3] , const bool hasPoints[3][3] , const int d[3] ) const 00995 { 00996 double v = 0.; 00997 const int min[] = { std::max<int>( 0 , d[0]+0 ) , std::max<int>( 0 , d[1]+0 ) , std::max<int>( 0 , d[2]+0 ) }; 00998 const int max[] = { std::min<int>( 2 , d[0]+2 ) , std::min<int>( 2 , d[1]+2 ) , std::min<int>( 2 , d[2]+2 ) }; 00999 for( int i=min[0] ; i<=max[0] ; i++ ) for( int j=min[1] ; j<=max[1] ; j++ ) 01000 { 01001 if( !hasPoints[i][j] ) continue; 01002 const PointInfo* pInfo = points[i][j]; 01003 int ii = -d[0]+i; 01004 int jj = -d[1]+j; 01005 for( int k=min[2] ; k<=max[2] ; k++ ) 01006 if( pInfo[k].weightedValue ) 01007 v += pInfo[k].splineValues[0][ii] * pInfo[k].splineValues[1][jj] * pInfo[k].splineValues[2][-d[2]+k]; 01008 } 01009 return Real( v ); 01010 } 01011 template<int Degree> 01012 Real Octree<Degree>::GetLaplacian( const int idx[DIMENSION] ) const 01013 { 01014 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]] ) ); 01015 } 01016 template< int Degree > 01017 Real Octree< Degree >::GetLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 ) const 01018 { 01019 int symIndex[] = 01020 { 01021 BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) , 01022 BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) , 01023 BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] ) 01024 }; 01025 return GetLaplacian( symIndex ); 01026 } 01027 template< int Degree > 01028 Real Octree< Degree >::GetDivergence( const TreeOctNode* node1 , const TreeOctNode* node2 , const Point3D< Real >& normal1 ) const 01029 { 01030 int symIndex[] = 01031 { 01032 BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) , 01033 BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) , 01034 BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] ) , 01035 }; 01036 int aSymIndex[] = 01037 { 01038 #if GRADIENT_DOMAIN_SOLUTION 01039 // Take the dot-product of the vector-field with the gradient of the basis function 01040 fData.Index( node2->off[0] , node1->off[0] ) , 01041 fData.Index( node2->off[1] , node1->off[1] ) , 01042 fData.Index( node2->off[2] , node1->off[2] ) 01043 #else // !GRADIENT_DOMAIN_SOLUTION 01044 // Take the dot-product of the divergence of the vector-field with the basis function 01045 fData.Index( node1->off[0] , node2->off[0] ) , 01046 fData.Index( node1->off[1] , node2->off[1] ) , 01047 fData.Index( node1->off[2] , node2->off[2] ) 01048 #endif // GRADIENT_DOMAIN_SOLUTION 01049 }; 01050 double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]]; 01051 #if GRADIENT_DOMAIN_SOLUTION 01052 return Real( dot * ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) ); 01053 #else // !GRADIENT_DOMAIN_SOLUTION 01054 return -Real( dot * ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) ); 01055 #endif // GRADIENT_DOMAIN_SOLUTION 01056 } 01057 template< int Degree > 01058 Real Octree< Degree >::GetDivergenceMinusLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 , Real value1 , const Point3D<Real>& normal1 ) const 01059 { 01060 int symIndex[] = 01061 { 01062 BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) , 01063 BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) , 01064 BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] ) 01065 }; 01066 int aSymIndex[] = 01067 { 01068 #if GRADIENT_DOMAIN_SOLUTION 01069 // Take the dot-product of the vector-field with the gradient of the basis function 01070 fData.Index( node2->off[0] , node1->off[0] ) , 01071 fData.Index( node2->off[1] , node1->off[1] ) , 01072 fData.Index( node2->off[2] , node1->off[2] ) 01073 #else // !GRADIENT_DOMAIN_SOLUTION 01074 // Take the dot-product of the divergence of the vector-field with the basis function 01075 fData.Index( node1->off[0] , node2->off[0] ) , 01076 fData.Index( node1->off[1] , node2->off[1] ) , 01077 fData.Index( node1->off[2] , node2->off[2] ) 01078 #endif // GRADIENT_DOMAIN_SOLUTION 01079 }; 01080 double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]]; 01081 return Real( dot * 01082 ( 01083 #if GRADIENT_DOMAIN_SOLUTION 01084 - ( fData.ddDotTable[ symIndex[0]] + fData.ddDotTable[ symIndex[1]] + fData.ddDotTable[ symIndex[2]] ) * value1 01085 + ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) 01086 #else // !GRADIENT_DOMAIN_SOLUTION 01087 - ( fData.ddDotTable[ symIndex[0]] + fData.ddDotTable[ symIndex[1]] + fData.ddDotTable[ symIndex[2]] ) * value1 01088 - ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) 01089 #endif // GRADIENT_DOMAIN_SOLUTION 01090 ) 01091 ); 01092 } 01093 template< int Degree > 01094 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 01095 { 01096 xStart = 0 , yStart = 0 , zStart = 0 , xEnd = 5 , yEnd = 5 , zEnd = 5; 01097 int depth , off[3]; 01098 node->depthAndOffset( depth , off ); 01099 int width = 1 << ( depth-rDepth ); 01100 off[0] -= rOff[0] << ( depth-rDepth ) , off[1] -= rOff[1] << ( depth-rDepth ) , off[2] -= rOff[2] << ( depth-rDepth ); 01101 if( off[0]<0 ) xStart = -off[0]; 01102 if( off[1]<0 ) yStart = -off[1]; 01103 if( off[2]<0 ) zStart = -off[2]; 01104 if( off[0]>=width ) xEnd = 4 - ( off[0]-width ); 01105 if( off[1]>=width ) yEnd = 4 - ( off[1]-width ); 01106 if( off[2]>=width ) zEnd = 4 - ( off[2]-width ); 01107 } 01108 template< int Degree > 01109 int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 ) const { return GetMatrixRowSize( neighbors5 , 0 , 5 , 0 , 5 , 0 , 5 ); } 01110 template< int Degree > 01111 int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const 01112 { 01113 int count = 0; 01114 for( int x=xStart ; x<=2 ; x++ ) 01115 for( int y=yStart ; y<yEnd ; y++ ) 01116 if( x==2 && y>2 ) continue; 01117 else for( int z=zStart ; z<zEnd ; z++ ) 01118 if( x==2 && y==2 && z>2 ) continue; 01119 else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 ) 01120 count++; 01121 return count; 01122 } 01123 template< int Degree > 01124 int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] ) const 01125 { 01126 return SetMatrixRow( neighbors5 , row , offset , stencil , 0 , 5 , 0 , 5 , 0 , 5 ); 01127 } 01128 template< int Degree > 01129 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 01130 { 01131 bool hasPoints[3][3]; 01132 Real diagonal = 0; 01133 PointInfo samples[3][3][3]; 01134 01135 int count = 0; 01136 const TreeOctNode* node = neighbors5.neighbors[2][2][2]; 01137 int index[] = { int( node->off[0] ) , int( node->off[1] ), int( node->off[2] ) }; 01138 01139 if( _constrainValues ) 01140 { 01141 int d , idx[3]; 01142 node->depthAndOffset( d , idx ); 01143 idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] ); 01144 idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] ); 01145 idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] ); 01146 for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) 01147 { 01148 hasPoints[j][k] = false; 01149 for( int l=0 ; l<3 ; l++ ) 01150 { 01151 const TreeOctNode* _node = neighbors5.neighbors[j+1][k+1][l+1]; 01152 if( _node && _node->nodeData.pointIndex!=-1 ) 01153 { 01154 const PointData& pData = _points[ _node->nodeData.pointIndex ]; 01155 PointInfo& pointInfo = samples[j][k][l]; 01156 Real weight = pData.weight; 01157 Point3D< Real > p = pData.position; 01158 for( int s=0 ; s<3 ; s++ ) 01159 { 01160 pointInfo.splineValues[0][s] = float( fData.baseBSplines[ idx[0]+j-s][s]( p[0] ) ); 01161 pointInfo.splineValues[1][s] = float( fData.baseBSplines[ idx[1]+k-s][s]( p[1] ) ); 01162 pointInfo.splineValues[2][s] = float( fData.baseBSplines[ idx[2]+l-s][s]( p[2] ) ); 01163 } 01164 float value = pointInfo.splineValues[0][j] * pointInfo.splineValues[1][k] * pointInfo.splineValues[2][l]; 01165 diagonal += value * value * weight; 01166 pointInfo.weightedValue = value * weight; 01167 for( int s=0 ; s<3 ; s++ ) pointInfo.splineValues[0][s] *= pointInfo.weightedValue; 01168 hasPoints[j][k] = true; 01169 } 01170 else samples[j][k][l].weightedValue = 0; 01171 } 01172 } 01173 } 01174 01175 bool isInterior; 01176 int d , off[3]; 01177 neighbors5.neighbors[2][2][2]->depthAndOffset( d , off ); 01178 int mn = 2 , mx = (1<<d)-2; 01179 isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx ); 01180 for( int x=xStart ; x<=2 ; x++ ) 01181 for( int y=yStart ; y<yEnd ; y++ ) 01182 if( x==2 && y>2 ) continue; 01183 else for( int z=zStart ; z<zEnd ; z++ ) 01184 if( x==2 && y==2 && z>2 ) continue; 01185 else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 ) 01186 { 01187 const TreeOctNode* _node = neighbors5.neighbors[x][y][z]; 01188 int _index[] = { int( _node->off[0] ) , int( _node->off[1] ), int( _node->off[2] ) }; 01189 Real temp; 01190 if( isInterior ) temp = Real( stencil[x][y][z] ); 01191 else temp = GetLaplacian( node , _node ); 01192 if( _constrainValues ) 01193 { 01194 int _d[] = { _index[0]-index[0] , _index[1]-index[1] , _index[2]-index[2] }; 01195 if( x==2 && y==2 && z==2 ) temp += diagonal; 01196 else temp += GetValue( samples , hasPoints , _d ); 01197 } 01198 if( x==2 && y==2 && z==2 ) temp /= 2; 01199 if( fabs(temp)>MATRIX_ENTRY_EPSILON ) 01200 { 01201 row[count].N = _node->nodeData.nodeIndex-offset; 01202 row[count].Value = temp; 01203 count++; 01204 } 01205 } 01206 return count; 01207 } 01208 template< int Degree > 01209 void Octree< Degree >::SetDivergenceStencil( int depth , Point3D< double > *stencil , bool scatter ) const 01210 { 01211 int offset[] = { 2 , 2 , 2 }; 01212 short d , off[3]; 01213 TreeOctNode::Index( depth , offset , d , off ); 01214 int index1[3] , index2[3]; 01215 if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] ); 01216 else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] ); 01217 for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ ) 01218 { 01219 int _offset[] = { x , y , z }; 01220 TreeOctNode::Index( depth , _offset , d , off ); 01221 if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] ); 01222 else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] ); 01223 int symIndex[] = 01224 { 01225 BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) , 01226 BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) , 01227 BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) , 01228 }; 01229 int aSymIndex[] = 01230 { 01231 #if GRADIENT_DOMAIN_SOLUTION 01232 // Take the dot-product of the vector-field with the gradient of the basis function 01233 fData.Index( index1[0] , index2[0] ) , 01234 fData.Index( index1[1] , index2[1] ) , 01235 fData.Index( index1[2] , index2[2] ) 01236 #else // !GRADIENT_DOMAIN_SOLUTION 01237 // Take the dot-product of the divergence of the vector-field with the basis function 01238 fData.Index( index2[0] , index1[0] ) , 01239 fData.Index( index2[1] , index1[1] ) , 01240 fData.Index( index2[2] , index1[2] ) 01241 #endif // GRADIENT_DOMAIN_SOLUTION 01242 }; 01243 01244 double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]]; 01245 #if GRADIENT_DOMAIN_SOLUTION 01246 Point3D<double> temp; 01247 temp[0] = fData.dvDotTable[aSymIndex[0]] * dot; 01248 temp[1] = fData.dvDotTable[aSymIndex[1]] * dot; 01249 temp[2] = fData.dvDotTable[aSymIndex[2]] * dot; 01250 stencil[25*x + 5*y + z] = temp; 01251 // stencil[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot; 01252 // stencil[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot; 01253 // stencil[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot; 01254 #else // !GRADIENT_DOMAIN_SOLUTION 01255 Point3D<double> temp; 01256 temp[0] = -fData.dvDotTable[aSymIndex[0]] * dot; 01257 temp[1] = -fData.dvDotTable[aSymIndex[1]] * dot; 01258 temp[2] = -fData.dvDotTable[aSymIndex[2]] * dot; 01259 stencil[25*x + 5*y + z] = temp; 01260 // stencil[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot; 01261 // stencil[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot; 01262 // stencil[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot; 01263 #endif // GRADIENT_DOMAIN_SOLUTION 01264 } 01265 } 01266 template< int Degree > 01267 void Octree< Degree >::SetLaplacianStencil( int depth , double stencil[5][5][5] ) const 01268 { 01269 int offset[] = { 2 , 2 , 2 }; 01270 short d , off[3]; 01271 TreeOctNode::Index( depth , offset , d , off ); 01272 int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) }; 01273 for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ ) 01274 { 01275 int _offset[] = { x , y , z }; 01276 short _d , _off[3]; 01277 TreeOctNode::Index( depth , _offset , _d , _off ); 01278 int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) }; 01279 int symIndex[3]; 01280 symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] ); 01281 symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] ); 01282 symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] ); 01283 stencil[x][y][z] = GetLaplacian( symIndex ); 01284 } 01285 } 01286 template< int Degree > 01287 void Octree< Degree >::SetLaplacianStencils( int depth , Stencil< double , 5 > stencils[2][2][2] ) const 01288 { 01289 if( depth<=1 ) return; 01290 for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) 01291 { 01292 short d , off[3]; 01293 int offset[] = { 4+i , 4+j , 4+k }; 01294 TreeOctNode::Index( depth , offset , d , off ); 01295 int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) }; 01296 for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ ) 01297 { 01298 int _offset[] = { x , y , z }; 01299 short _d , _off[3]; 01300 TreeOctNode::Index( depth-1 , _offset , _d , _off ); 01301 int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) }; 01302 int symIndex[3]; 01303 symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] ); 01304 symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] ); 01305 symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] ); 01306 stencils[i][j][k].values[x][y][z] = GetLaplacian( symIndex ); 01307 } 01308 } 01309 } 01310 template< int Degree > 01311 void Octree< Degree >::SetDivergenceStencils( int depth , Stencil< Point3D< double > , 5 > stencils[2][2][2] , bool scatter ) const 01312 { 01313 if( depth<=1 ) return; 01314 int index1[3] , index2[3]; 01315 for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) 01316 { 01317 short d , off[3]; 01318 int offset[] = { 4+i , 4+j , 4+k }; 01319 TreeOctNode::Index( depth , offset , d , off ); 01320 if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] ); 01321 else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] ); 01322 for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ ) 01323 { 01324 int _offset[] = { x , y , z }; 01325 TreeOctNode::Index( depth-1 , _offset , d , off ); 01326 if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] ); 01327 else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] ); 01328 01329 int symIndex[] = 01330 { 01331 BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) , 01332 BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) , 01333 BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) , 01334 }; 01335 int aSymIndex[] = 01336 { 01337 #if GRADIENT_DOMAIN_SOLUTION 01338 // Take the dot-product of the vector-field with the gradient of the basis function 01339 fData.Index( index1[0] , index2[0] ) , 01340 fData.Index( index1[1] , index2[1] ) , 01341 fData.Index( index1[2] , index2[2] ) 01342 #else // !GRADIENT_DOMAIN_SOLUTION 01343 // Take the dot-product of the divergence of the vector-field with the basis function 01344 fData.Index( index2[0] , index1[0] ) , 01345 fData.Index( index2[1] , index1[1] ) , 01346 fData.Index( index2[2] , index1[2] ) 01347 #endif // GRADIENT_DOMAIN_SOLUTION 01348 }; 01349 double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]]; 01350 #if GRADIENT_DOMAIN_SOLUTION 01351 stencils[i][j][k].values[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot; 01352 stencils[i][j][k].values[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot; 01353 stencils[i][j][k].values[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot; 01354 #else // !GRADIENT_DOMAIN_SOLUTION 01355 stencils[i][j][k].values[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot; 01356 stencils[i][j][k].values[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot; 01357 stencils[i][j][k].values[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot; 01358 #endif // GRADIENT_DOMAIN_SOLUTION 01359 } 01360 } 01361 } 01362 template< int Degree > 01363 void Octree< Degree >::SetEvaluationStencils( int depth , Stencil< double , 3 > stencil1[8] , Stencil< double , 3 > stencil2[8][8] ) const 01364 { 01365 if( depth>2 ) 01366 { 01367 int idx[3]; 01368 int off[] = { 2 , 2 , 2 }; 01369 for( int c=0 ; c<8 ; c++ ) 01370 { 01371 VertexData::CornerIndex( depth , off , c , fData.depth , idx ); 01372 idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount; 01373 for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ ) 01374 { 01375 short _d , _off[3]; 01376 int _offset[] = { x+1 , y+1 , z+1 }; 01377 TreeOctNode::Index( depth , _offset , _d , _off ); 01378 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]) ]; 01379 } 01380 } 01381 } 01382 if( depth>3 ) 01383 for( int _c=0 ; _c<8 ; _c++ ) 01384 { 01385 int idx[3]; 01386 int _cx , _cy , _cz; 01387 Cube::FactorCornerIndex( _c , _cx , _cy , _cz ); 01388 int off[] = { 4+_cx , 4+_cy , 4+_cz }; 01389 for( int c=0 ; c<8 ; c++ ) 01390 { 01391 VertexData::CornerIndex( depth , off , c , fData.depth , idx ); 01392 idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount; 01393 for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ ) 01394 { 01395 short _d , _off[3]; 01396 int _offset[] = { x+1 , y+1 , z+1 }; 01397 TreeOctNode::Index( depth-1 , _offset , _d , _off ); 01398 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]) ]; 01399 } 01400 } 01401 } 01402 } 01403 template< int Degree > 01404 void Octree< Degree >::UpdateCoarserSupportBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ ) 01405 { 01406 if( node->parent ) 01407 { 01408 int x , y , z , c = int( node - node->parent->children ); 01409 Cube::FactorCornerIndex( c , x , y , z ); 01410 if( x==0 ) endX = 4; 01411 else startX = 1; 01412 if( y==0 ) endY = 4; 01413 else startY = 1; 01414 if( z==0 ) endZ = 4; 01415 else startZ = 1; 01416 } 01417 } 01418 01419 template< int Degree > 01420 void Octree< Degree >::UpdateConstraintsFromCoarser( const OctNode< TreeNodeData , Real >::NeighborKey5& neighborKey5 , TreeOctNode* node , Real* metSolution , const Stencil< double , 5 >& lapStencil ) const 01421 { 01422 bool isInterior; 01423 { 01424 int d , off[3]; 01425 node->depthAndOffset( d , off ); 01426 int mn = 4 , mx = (1<<d)-4; 01427 isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx ); 01428 } 01429 Real constraint = Real( 0 ); 01430 int depth = node->depth(); 01431 if( depth<=_minDepth ) return; 01432 int i = node->nodeData.nodeIndex; 01433 // Offset the constraints using the solution from lower resolutions. 01434 int startX = 0 , endX = 5 , startY = 0 , endY = 5 , startZ = 0 , endZ = 5; 01435 UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ ); 01436 01437 const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth-1]; 01438 for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ ) 01439 if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 ) 01440 { 01441 const TreeOctNode* _node = neighbors5.neighbors[x][y][z]; 01442 Real _solution = metSolution[ _node->nodeData.nodeIndex ]; 01443 { 01444 if( isInterior ) node->nodeData.constraint -= Real( lapStencil.values[x][y][z] * _solution ); 01445 else node->nodeData.constraint -= GetLaplacian( _node , node ) * _solution; 01446 } 01447 } 01448 if( _constrainValues ) 01449 { 01450 int d , idx[3]; 01451 node->depthAndOffset( d, idx ); 01452 idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] ); 01453 idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] ); 01454 idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] ); 01455 const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth]; 01456 for( int x=1 ; x<4 ; x++ ) for( int y=1 ; y<4 ; y++ ) for( int z=1 ; z<4 ; z++ ) 01457 if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.pointIndex!=-1 ) 01458 { 01459 const PointData& pData = _points[ neighbors5.neighbors[x][y][z]->nodeData.pointIndex ]; 01460 Real pointValue = pData.value; 01461 Point3D< Real > p = pData.position; 01462 node->nodeData.constraint -= 01463 Real( 01464 fData.baseBSplines[idx[0]][x-1]( p[0] ) * 01465 fData.baseBSplines[idx[1]][y-1]( p[1] ) * 01466 fData.baseBSplines[idx[2]][z-1]( p[2] ) * 01467 pointValue 01468 ); 01469 } 01470 } 01471 } 01472 struct UpSampleData 01473 { 01474 int start; 01475 double v[2]; 01476 UpSampleData( void ) { start = 0 , v[0] = v[1] = 0.; } 01477 UpSampleData( int s , double v1 , double v2 ) { start = s , v[0] = v1 , v[1] = v2; } 01478 }; 01479 template< int Degree > 01480 void Octree< Degree >::UpSampleCoarserSolution( int depth , const SortedTreeNodes& sNodes , Vector< Real >& Solution ) const 01481 { 01482 int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start; 01483 Solution.Resize( range ); 01484 if( !depth ) return; 01485 else if( depth==1 ) for( int i=start ; i<end ; i++ ) Solution[i-start] += sNodes.treeNodes[0]->nodeData.solution; 01486 else 01487 { 01488 // For every node at the current depth 01489 #pragma omp parallel for num_threads( threads ) 01490 for( int t=0 ; t<threads ; t++ ) 01491 { 01492 TreeOctNode::NeighborKey3 neighborKey; 01493 neighborKey.set( depth ); 01494 for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ ) 01495 { 01496 int d , off[3]; 01497 UpSampleData usData[3]; 01498 sNodes.treeNodes[i]->depthAndOffset( d , off ); 01499 for( int d=0 ; d<3 ; d++ ) 01500 { 01501 if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 ); 01502 else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 ); 01503 else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 ); 01504 else usData[d] = UpSampleData( 0 , 0.25 , 0.75 ); 01505 } 01506 neighborKey.getNeighbors( sNodes.treeNodes[i] ); 01507 for( int ii=0 ; ii<2 ; ii++ ) 01508 { 01509 int _ii = ii + usData[0].start; 01510 double dx = usData[0].v[ii]; 01511 for( int jj=0 ; jj<2 ; jj++ ) 01512 { 01513 int _jj = jj + usData[1].start; 01514 double dxy = dx * usData[1].v[jj]; 01515 for( int kk=0 ; kk<2 ; kk++ ) 01516 { 01517 int _kk = kk + usData[2].start; 01518 double dxyz = dxy * usData[2].v[kk]; 01519 Solution[i-start] += Real( neighborKey.neighbors[depth-1].neighbors[_ii][_jj][_kk]->nodeData.solution * dxyz ); 01520 } 01521 } 01522 } 01523 } 01524 } 01525 } 01526 // Clear the coarser solution 01527 start = sNodes.nodeCount[depth-1] , end = sNodes.nodeCount[depth] , range = end-start; 01528 #pragma omp parallel for num_threads( threads ) 01529 for( int i=start ; i<end ; i++ ) sNodes.treeNodes[i]->nodeData.solution = Real( 0. ); 01530 } 01531 template< int Degree > 01532 void Octree< Degree >::DownSampleFinerConstraints( int depth , SortedTreeNodes& sNodes ) const 01533 { 01534 if( !depth ) return; 01535 #pragma omp parallel for num_threads( threads ) 01536 for( int i=sNodes.nodeCount[depth-1] ; i<sNodes.nodeCount[depth] ; i++ ) 01537 sNodes.treeNodes[i]->nodeData.constraint = Real( 0 ); 01538 01539 if( depth==1 ) 01540 { 01541 sNodes.treeNodes[0]->nodeData.constraint = Real( 0 ); 01542 for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) sNodes.treeNodes[0]->nodeData.constraint += sNodes.treeNodes[i]->nodeData.constraint; 01543 return; 01544 } 01545 std::vector< Vector< double > > constraints( threads ); 01546 for( int t=0 ; t<threads ; t++ ) constraints[t].Resize( sNodes.nodeCount[depth] - sNodes.nodeCount[depth-1] ) , constraints[t].SetZero(); 01547 int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start; 01548 int lStart = sNodes.nodeCount[depth-1] , lEnd = sNodes.nodeCount[depth]; 01549 // For every node at the current depth 01550 #pragma omp parallel for num_threads( threads ) 01551 for( int t=0 ; t<threads ; t++ ) 01552 { 01553 TreeOctNode::NeighborKey3 neighborKey; 01554 neighborKey.set( depth ); 01555 for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ ) 01556 { 01557 int d , off[3]; 01558 UpSampleData usData[3]; 01559 sNodes.treeNodes[i]->depthAndOffset( d , off ); 01560 for( int d=0 ; d<3 ; d++ ) 01561 { 01562 if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 ); 01563 else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 ); 01564 else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 ); 01565 else usData[d] = UpSampleData( 0 , 0.25 , 0.75 ); 01566 } 01567 neighborKey.getNeighbors( sNodes.treeNodes[i] ); 01568 TreeOctNode::Neighbors3& neighbors = neighborKey.neighbors[depth-1]; 01569 for( int ii=0 ; ii<2 ; ii++ ) 01570 { 01571 int _ii = ii + usData[0].start; 01572 double dx = usData[0].v[ii]; 01573 for( int jj=0 ; jj<2 ; jj++ ) 01574 { 01575 int _jj = jj + usData[1].start; 01576 double dxy = dx * usData[1].v[jj]; 01577 for( int kk=0 ; kk<2 ; kk++ ) 01578 { 01579 int _kk = kk + usData[2].start; 01580 double dxyz = dxy * usData[2].v[kk]; 01581 constraints[t][neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex-lStart] += sNodes.treeNodes[i]->nodeData.constraint * dxyz; 01582 } 01583 } 01584 } 01585 } 01586 } 01587 #pragma omp parallel for num_threads( threads ) 01588 for( int i=lStart ; i<lEnd ; i++ ) 01589 { 01590 Real cSum = Real(0.); 01591 for( int t=0 ; t<threads ; t++ ) cSum += constraints[t][i-lStart]; 01592 sNodes.treeNodes[i]->nodeData.constraint += cSum; 01593 } 01594 } 01595 template< int Degree > 01596 template< class C > 01597 void Octree< Degree >::DownSample( int depth , const SortedTreeNodes& sNodes , C* constraints ) const 01598 { 01599 if( depth==0 ) return; 01600 if( depth==1 ) 01601 { 01602 for( int i=sNodes.nodeCount[1] ; i<sNodes.nodeCount[2] ; i++ ) constraints[0] += constraints[i]; 01603 return; 01604 } 01605 std::vector< Vector< C > > _constraints( threads ); 01606 for( int t=0 ; t<threads ; t++ ) _constraints[t].Resize( sNodes.nodeCount[depth] - sNodes.nodeCount[depth-1] ); 01607 int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start , lStart = sNodes.nodeCount[depth-1] , lEnd = sNodes.nodeCount[depth]; 01608 // For every node at the current depth 01609 #pragma omp parallel for num_threads( threads ) 01610 for( int t=0 ; t<threads ; t++ ) 01611 { 01612 TreeOctNode::NeighborKey3 neighborKey; 01613 neighborKey.set( depth ); 01614 for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ ) 01615 { 01616 int d , off[3]; 01617 UpSampleData usData[3]; 01618 sNodes.treeNodes[i]->depthAndOffset( d , off ); 01619 for( int d=0 ; d<3 ; d++ ) 01620 { 01621 if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 ); 01622 else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 ); 01623 else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 ); 01624 else usData[d] = UpSampleData( 0 , 0.25 , 0.75 ); 01625 } 01626 TreeOctNode::Neighbors3& neighbors = neighborKey.getNeighbors( sNodes.treeNodes[i]->parent ); 01627 C c = constraints[i]; 01628 for( int ii=0 ; ii<2 ; ii++ ) 01629 { 01630 int _ii = ii + usData[0].start; 01631 C cx = C( c*usData[0].v[ii] ); 01632 for( int jj=0 ; jj<2 ; jj++ ) 01633 { 01634 int _jj = jj + usData[1].start; 01635 C cxy = C( cx*usData[1].v[jj] ); 01636 for( int kk=0 ; kk<2 ; kk++ ) 01637 { 01638 int _kk = kk + usData[2].start; 01639 if( neighbors.neighbors[_ii][_jj][_kk] ) 01640 _constraints[t][neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex-lStart] += C( cxy*usData[2].v[kk] ); 01641 } 01642 } 01643 } 01644 } 01645 } 01646 #pragma omp parallel for num_threads( threads ) 01647 for( int i=lStart ; i<lEnd ; i++ ) 01648 { 01649 C cSum = C(0); 01650 for( int t=0 ; t<threads ; t++ ) cSum += _constraints[t][i-lStart]; 01651 constraints[i] += cSum; 01652 } 01653 } 01654 template< int Degree > 01655 template< class C > 01656 void Octree< Degree >::UpSample( int depth , const SortedTreeNodes& sNodes , C* coefficients ) const 01657 { 01658 if ( depth==0 ) return; 01659 else if( depth==1 ) 01660 { 01661 for( int i=sNodes.nodeCount[1] ; i<sNodes.nodeCount[2] ; i++ ) coefficients[i] += coefficients[0]; 01662 return; 01663 } 01664 01665 int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start; 01666 // For every node at the current depth 01667 #pragma omp parallel for num_threads( threads ) 01668 for( int t=0 ; t<threads ; t++ ) 01669 { 01670 TreeOctNode::NeighborKey3 neighborKey; 01671 neighborKey.set( depth-1 ); 01672 for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ ) 01673 { 01674 TreeOctNode* node = sNodes.treeNodes[i]; 01675 int d , off[3]; 01676 UpSampleData usData[3]; 01677 node->depthAndOffset( d , off ); 01678 for( int d=0 ; d<3 ; d++ ) 01679 { 01680 if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 ); 01681 else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 ); 01682 else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 ); 01683 else usData[d] = UpSampleData( 0 , 0.25 , 0.75 ); 01684 } 01685 TreeOctNode::Neighbors3& neighbors = neighborKey.getNeighbors( node->parent ); 01686 for( int ii=0 ; ii<2 ; ii++ ) 01687 { 01688 int _ii = ii + usData[0].start; 01689 double dx = usData[0].v[ii]; 01690 for( int jj=0 ; jj<2 ; jj++ ) 01691 { 01692 int _jj = jj + usData[1].start; 01693 double dxy = dx * usData[1].v[jj]; 01694 for( int kk=0 ; kk<2 ; kk++ ) 01695 { 01696 int _kk = kk + usData[2].start; 01697 if( neighbors.neighbors[_ii][_jj][_kk] ) 01698 { 01699 double dxyz = dxy * usData[2].v[kk]; 01700 int _i = neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex; 01701 coefficients[i] += coefficients[_i] * Real( dxyz ); 01702 } 01703 } 01704 } 01705 } 01706 } 01707 } 01708 } 01709 template< int Degree > 01710 void Octree< Degree >::SetCoarserPointValues( int depth , const SortedTreeNodes& sNodes , Real* metSolution ) 01711 { 01712 int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start; 01713 // For every node at the current depth 01714 #pragma omp parallel for num_threads( threads ) 01715 for( int t=0 ; t<threads ; t++ ) 01716 { 01717 TreeOctNode::NeighborKey3 neighborKey; 01718 neighborKey.set( depth ); 01719 for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ ) 01720 { 01721 int pIdx = sNodes.treeNodes[i]->nodeData.pointIndex; 01722 if( pIdx!=-1 ) 01723 { 01724 neighborKey.getNeighbors( sNodes.treeNodes[i] ); 01725 _points[ pIdx ].value = WeightedCoarserFunctionValue( neighborKey , sNodes.treeNodes[i] , metSolution ); 01726 } 01727 } 01728 } 01729 } 01730 template< int Degree > 01731 Real Octree< Degree >::WeightedCoarserFunctionValue( const OctNode< TreeNodeData , Real >::NeighborKey3& neighborKey , const TreeOctNode* pointNode , Real* metSolution ) const 01732 { 01733 int depth = pointNode->depth(); 01734 if( !depth || pointNode->nodeData.pointIndex==-1 ) return Real(0.); 01735 double pointValue = 0; 01736 01737 Real weight = _points[ pointNode->nodeData.pointIndex ].weight; 01738 Point3D< Real > p = _points[ pointNode->nodeData.pointIndex ].position; 01739 01740 // Iterate over all basis functions that overlap the point at the coarser resolutions 01741 { 01742 int d , _idx[3]; 01743 const TreeOctNode::Neighbors3& neighbors = neighborKey.neighbors[depth-1]; 01744 neighbors.neighbors[1][1][1]->depthAndOffset( d , _idx ); 01745 _idx[0] = BinaryNode< double >::CenterIndex( d , _idx[0]-1 ); 01746 _idx[1] = BinaryNode< double >::CenterIndex( d , _idx[1]-1 ); 01747 _idx[2] = BinaryNode< double >::CenterIndex( d , _idx[2]-1 ); 01748 for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) for( int l=0 ; l<3 ; l++ ) 01749 if( neighbors.neighbors[j][k][l] && neighbors.neighbors[j][k][l]->nodeData.nodeIndex>=0 ) 01750 { 01751 // Accumulate the contribution from these basis nodes 01752 const TreeOctNode* basisNode = neighbors.neighbors[j][k][l]; 01753 int idx[] = { _idx[0]+j , _idx[1]+k , _idx[2]+l }; 01754 pointValue += 01755 fData.baseBSplines[ idx[0] ][2-j]( p[0] ) * 01756 fData.baseBSplines[ idx[1] ][2-k]( p[1] ) * 01757 fData.baseBSplines[ idx[2] ][2-l]( p[2] ) * 01758 metSolution[basisNode->nodeData.nodeIndex]; 01759 } 01760 } 01761 return Real( pointValue * weight ); 01762 } 01763 template<int Degree> 01764 int Octree<Degree>::GetFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix , int depth , const SortedTreeNodes& sNodes , Real* metSolution ) 01765 { 01766 int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start; 01767 double stencil[5][5][5]; 01768 SetLaplacianStencil( depth , stencil ); 01769 Stencil< double , 5 > stencils[2][2][2]; 01770 SetLaplacianStencils( depth , stencils ); 01771 matrix.Resize( range ); 01772 #pragma omp parallel for num_threads( threads ) 01773 for( int t=0 ; t<threads ; t++ ) 01774 { 01775 TreeOctNode::NeighborKey5 neighborKey5; 01776 neighborKey5.set( depth ); 01777 for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ ) 01778 { 01779 TreeOctNode* node = sNodes.treeNodes[i+start]; 01780 neighborKey5.getNeighbors( node ); 01781 01782 // Get the matrix row size 01783 int count = GetMatrixRowSize( neighborKey5.neighbors[depth] ); 01784 01785 // Allocate memory for the row 01786 #pragma omp critical (matrix_set_row_size) 01787 { 01788 matrix.SetRowSize( i , count ); 01789 } 01790 01791 // Set the row entries 01792 matrix.rowSizes[i] = SetMatrixRow( neighborKey5.neighbors[depth] , matrix[i] , start , stencil ); 01793 01794 // Offset the constraints using the solution from lower resolutions. 01795 int x , y , z , c; 01796 if( node->parent ) 01797 { 01798 c = int( node - node->parent->children ); 01799 Cube::FactorCornerIndex( c , x , y , z ); 01800 } 01801 else x = y = z = 0; 01802 UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] ); 01803 } 01804 } 01805 return 1; 01806 } 01807 template<int Degree> 01808 int Octree<Degree>::GetRestrictedFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix,int depth,const int* entries,int entryCount, 01809 const TreeOctNode* rNode , Real radius , 01810 const SortedTreeNodes& sNodes , Real* metSolution ) 01811 { 01812 for( int i=0 ; i<entryCount ; i++ ) sNodes.treeNodes[ entries[i] ]->nodeData.nodeIndex = i; 01813 double stencil[5][5][5]; 01814 int rDepth , rOff[3]; 01815 rNode->depthAndOffset( rDepth , rOff ); 01816 matrix.Resize( entryCount ); 01817 SetLaplacianStencil( depth , stencil ); 01818 Stencil< double , 5 > stencils[2][2][2]; 01819 SetLaplacianStencils( depth , stencils ); 01820 #pragma omp parallel for num_threads( threads ) 01821 for( int t=0 ; t<threads ; t++ ) 01822 { 01823 TreeOctNode::NeighborKey5 neighborKey5; 01824 neighborKey5.set( depth ); 01825 for( int i=(entryCount*t)/threads ; i<(entryCount*(t+1))/threads ; i++ ) 01826 { 01827 TreeOctNode* node = sNodes.treeNodes[ entries[i] ]; 01828 int d , off[3]; 01829 node->depthAndOffset( d , off ); 01830 off[0] >>= (depth-rDepth) , off[1] >>= (depth-rDepth) , off[2] >>= (depth-rDepth); 01831 bool isInterior = ( off[0]==rOff[0] && off[1]==rOff[1] && off[2]==rOff[2] ); 01832 01833 neighborKey5.getNeighbors( node ); 01834 01835 int xStart=0 , xEnd=5 , yStart=0 , yEnd=5 , zStart=0 , zEnd=5; 01836 if( !isInterior ) SetMatrixRowBounds( neighborKey5.neighbors[depth].neighbors[2][2][2] , rDepth , rOff , xStart , xEnd , yStart , yEnd , zStart , zEnd ); 01837 01838 // Get the matrix row size 01839 int count = GetMatrixRowSize( neighborKey5.neighbors[depth] , xStart , xEnd , yStart , yEnd , zStart , zEnd ); 01840 01841 // Allocate memory for the row 01842 #pragma omp critical (matrix_set_row_size) 01843 { 01844 matrix.SetRowSize( i , count ); 01845 } 01846 01847 // Set the matrix row entries 01848 matrix.rowSizes[i] = SetMatrixRow( neighborKey5.neighbors[depth] , matrix[i] , 0 , stencil , xStart , xEnd , yStart , yEnd , zStart , zEnd ); 01849 // Adjust the system constraints 01850 int x , y , z , c; 01851 if( node->parent ) 01852 { 01853 c = int( node - node->parent->children ); 01854 Cube::FactorCornerIndex( c , x , y , z ); 01855 } 01856 else x = y = z = 0; 01857 UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] ); 01858 } 01859 } 01860 for( int i=0 ; i<entryCount ; i++ ) sNodes.treeNodes[entries[i]]->nodeData.nodeIndex = entries[i]; 01861 return 1; 01862 } 01863 01864 template<int Degree> 01865 int Octree<Degree>::LaplacianMatrixIteration( int subdivideDepth , bool showResidual , int minIters , double accuracy ) 01866 { 01867 int i,iter=0; 01868 double t = 0; 01869 fData.setDotTables( fData.DD_DOT_FLAG | fData.DV_DOT_FLAG ); 01870 01871 SparseMatrix< float >::SetAllocator( MEMORY_ALLOCATOR_BLOCK_SIZE ); 01872 _sNodes.treeNodes[0]->nodeData.solution = 0; 01873 01874 std::vector< Real > metSolution( _sNodes.nodeCount[ _sNodes.maxDepth ] , 0 ); 01875 01876 for( i=1 ; i<_sNodes.maxDepth ; i++ ) 01877 { 01878 if( subdivideDepth>0 ) iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , subdivideDepth , showResidual , minIters , accuracy ); 01879 else iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , showResidual , minIters , accuracy ); 01880 } 01881 SparseMatrix< float >::internalAllocator.reset(); 01882 fData.clearDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG | fData.DD_DOT_FLAG ); 01883 01884 return iter; 01885 } 01886 01887 template<int Degree> 01888 int Octree<Degree>::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , bool showResidual , int minIters , double accuracy ) 01889 { 01890 int iter = 0; 01891 Vector< Real > X , B; 01892 SparseSymmetricMatrix< Real > M; 01893 double systemTime=0. , solveTime=0. , updateTime=0. , evaluateTime = 0.; 01894 01895 X.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] ); 01896 if( depth<=_minDepth ) UpSampleCoarserSolution( depth , sNodes , X ); 01897 else 01898 { 01899 // Up-sample the cumulative solution into the previous depth 01900 UpSample( depth-1 , sNodes , metSolution ); 01901 // Add in the solution from that depth 01902 if( depth ) 01903 #pragma omp parallel for num_threads( threads ) 01904 for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution; 01905 } 01906 if( _constrainValues ) 01907 { 01908 SetCoarserPointValues( depth , sNodes , metSolution ); 01909 } 01910 01911 SparseSymmetricMatrix< Real >::internalAllocator.rollBack(); 01912 { 01913 int maxECount = ( (2*Degree+1)*(2*Degree+1)*(2*Degree+1) + 1 ) / 2; 01914 maxECount = ( ( maxECount + 15 ) / 16 ) * 16; 01915 M.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] ); 01916 for( int i=0 ; i<M.rows ; i++ ) M.SetRowSize( i , maxECount ); 01917 } 01918 01919 { 01920 // Get the system matrix 01921 SparseSymmetricMatrix< Real >::internalAllocator.rollBack(); 01922 GetFixedDepthLaplacian( M , depth , sNodes , metSolution ); 01923 // Set the constraint vector 01924 B.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] ); 01925 for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) B[i-sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.constraint; 01926 } 01927 01928 // Solve the linear system 01929 iter += SparseSymmetricMatrix< Real >::Solve( M , B , std::max< int >( int( pow( M.rows , ITERATION_POWER ) ) , minIters ) , X , Real(accuracy) , 0 , threads , (depth<=_minDepth) && !_constrainValues ); 01930 01931 if( showResidual ) 01932 { 01933 double mNorm = 0; 01934 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; 01935 double bNorm = B.Norm( 2 ) , rNorm = ( B - M * X ).Norm( 2 ); 01936 printf( "\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter ); 01937 } 01938 01939 // Copy the solution back into the tree (over-writing the constraints) 01940 for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) sNodes.treeNodes[i]->nodeData.solution = Real( X[i-sNodes.nodeCount[depth]] ); 01941 01942 return iter; 01943 } 01944 template<int Degree> 01945 int Octree<Degree>::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , int startingDepth , bool showResidual , int minIters , double accuracy ) 01946 { 01947 if( startingDepth>=depth ) return SolveFixedDepthMatrix( depth , sNodes , metSolution , showResidual , minIters , accuracy ); 01948 01949 int i , j , d , tIter=0; 01950 SparseSymmetricMatrix< Real > _M; 01951 Vector< Real > B , _B , _X; 01952 AdjacencySetFunction asf; 01953 AdjacencyCountFunction acf; 01954 double systemTime = 0 , solveTime = 0 , memUsage = 0 , evaluateTime = 0 , gTime = 0, sTime = 0; 01955 Real myRadius , myRadius2; 01956 01957 if( depth>_minDepth ) 01958 { 01959 // Up-sample the cumulative solution into the previous depth 01960 UpSample( depth-1 , sNodes , metSolution ); 01961 // Add in the solution from that depth 01962 if( depth ) 01963 #pragma omp parallel for num_threads( threads ) 01964 for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution; 01965 } 01966 01967 if( _constrainValues ) 01968 { 01969 SetCoarserPointValues( depth , sNodes , metSolution ); 01970 } 01971 B.Resize( sNodes.nodeCount[depth+1] - sNodes.nodeCount[depth] ); 01972 01973 // Back-up the constraints 01974 for( i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) 01975 { 01976 B[ i-sNodes.nodeCount[depth] ] = sNodes.treeNodes[i]->nodeData.constraint; 01977 sNodes.treeNodes[i]->nodeData.constraint = 0; 01978 } 01979 01980 myRadius = 2*radius-Real(0.5); 01981 myRadius = int(myRadius-ROUND_EPS)+ROUND_EPS; 01982 myRadius2 = Real(radius+ROUND_EPS-0.5); 01983 d = depth-startingDepth; 01984 std::vector< int > subDimension( sNodes.nodeCount[d+1]-sNodes.nodeCount[d] ); 01985 int maxDimension = 0; 01986 for( i=sNodes.nodeCount[d] ; i<sNodes.nodeCount[d+1] ; i++ ) 01987 { 01988 // Count the number of nodes at depth "depth" that lie under sNodes.treeNodes[i] 01989 acf.adjacencyCount = 0; 01990 for( TreeOctNode* temp=sNodes.treeNodes[i]->nextNode() ; temp ; ) 01991 { 01992 if( temp->depth()==depth ) acf.adjacencyCount++ , temp = sNodes.treeNodes[i]->nextBranch( temp ); 01993 else temp = sNodes.treeNodes[i]->nextNode ( temp ); 01994 } 01995 for( j=sNodes.nodeCount[d] ; j<sNodes.nodeCount[d+1] ; j++ ) 01996 { 01997 if( i==j ) continue; 01998 TreeOctNode::ProcessFixedDepthNodeAdjacentNodes( fData.depth , sNodes.treeNodes[i] , 1 , sNodes.treeNodes[j] , 2*width-1 , depth , &acf ); 01999 } 02000 subDimension[i-sNodes.nodeCount[d]] = acf.adjacencyCount; 02001 maxDimension = std::max< int >( maxDimension , subDimension[i-sNodes.nodeCount[d]] ); 02002 } 02003 asf.adjacencies = new int[maxDimension]; 02004 MapReduceVector< Real > mrVector; 02005 mrVector.resize( threads , maxDimension ); 02006 // Iterate through the coarse-level nodes 02007 for( i=sNodes.nodeCount[d] ; i<sNodes.nodeCount[d+1] ; i++ ) 02008 { 02009 int iter = 0; 02010 // Count the number of nodes at depth "depth" that lie under sNodes.treeNodes[i] 02011 acf.adjacencyCount = subDimension[i-sNodes.nodeCount[d]]; 02012 if( !acf.adjacencyCount ) continue; 02013 02014 // Set the indices for the nodes under, or near, sNodes.treeNodes[i]. 02015 asf.adjacencyCount = 0; 02016 for( TreeOctNode* temp=sNodes.treeNodes[i]->nextNode() ; temp ; ) 02017 { 02018 if( temp->depth()==depth ) asf.adjacencies[ asf.adjacencyCount++ ] = temp->nodeData.nodeIndex , temp = sNodes.treeNodes[i]->nextBranch( temp ); 02019 else temp = sNodes.treeNodes[i]->nextNode ( temp ); 02020 } 02021 for( j=sNodes.nodeCount[d] ; j<sNodes.nodeCount[d+1] ; j++ ) 02022 { 02023 if( i==j ) continue; 02024 TreeOctNode::ProcessFixedDepthNodeAdjacentNodes( fData.depth , sNodes.treeNodes[i] , 1 , sNodes.treeNodes[j] , 2*width-1 , depth , &asf ); 02025 } 02026 02027 // Get the associated constraint vector 02028 _B.Resize( asf.adjacencyCount ); 02029 for( j=0 ; j<asf.adjacencyCount ; j++ ) _B[j] = B[ asf.adjacencies[j]-sNodes.nodeCount[depth] ]; 02030 02031 _X.Resize( asf.adjacencyCount ); 02032 #pragma omp parallel for num_threads( threads ) schedule( static ) 02033 for( j=0 ; j<asf.adjacencyCount ; j++ ) 02034 { 02035 _X[j] = sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution; 02036 } 02037 // Get the associated matrix 02038 SparseSymmetricMatrix< Real >::internalAllocator.rollBack(); 02039 GetRestrictedFixedDepthLaplacian( _M , depth , asf.adjacencies , asf.adjacencyCount , sNodes.treeNodes[i] , myRadius , sNodes , metSolution ); 02040 #pragma omp parallel for num_threads( threads ) schedule( static ) 02041 for( j=0 ; j<asf.adjacencyCount ; j++ ) 02042 { 02043 _B[j] += sNodes.treeNodes[asf.adjacencies[j]]->nodeData.constraint; 02044 sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.constraint = 0; 02045 } 02046 02047 // Solve the matrix 02048 // Since we don't have the full matrix, the system shouldn't be singular, so we shouldn't have to correct it 02049 iter += SparseSymmetricMatrix< Real >::Solve( _M , _B , std::max< int >( int( pow( _M.rows , ITERATION_POWER ) ) , minIters ) , _X , mrVector , Real(accuracy) , 0 ); 02050 02051 if( showResidual ) 02052 { 02053 double mNorm = 0; 02054 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; 02055 double bNorm = _B.Norm( 2 ) , rNorm = ( _B - _M * _X ).Norm( 2 ); 02056 printf( "\t\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , _M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter ); 02057 } 02058 02059 // Update the solution for all nodes in the sub-tree 02060 for( j=0 ; j<asf.adjacencyCount ; j++ ) 02061 { 02062 TreeOctNode* temp=sNodes.treeNodes[ asf.adjacencies[j] ]; 02063 while( temp->depth()>sNodes.treeNodes[i]->depth() ) temp=temp->parent; 02064 if( temp->nodeData.nodeIndex>=sNodes.treeNodes[i]->nodeData.nodeIndex ) sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution = Real( _X[j] ); 02065 } 02066 systemTime += gTime; 02067 solveTime += sTime; 02068 memUsage = std::max< double >( MemoryUsage() , memUsage ); 02069 tIter += iter; 02070 } 02071 delete[] asf.adjacencies; 02072 return tIter; 02073 } 02074 template<int Degree> 02075 int Octree<Degree>::HasNormals(TreeOctNode* node,Real epsilon) 02076 { 02077 int hasNormals=0; 02078 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; 02079 if( node->children ) for(int i=0;i<Cube::CORNERS && !hasNormals;i++) hasNormals |= HasNormals(&node->children[i],epsilon); 02080 02081 return hasNormals; 02082 } 02083 template<int Degree> 02084 void Octree<Degree>::ClipTree( void ) 02085 { 02086 int maxDepth = tree.maxDepth(); 02087 for( TreeOctNode* temp=tree.nextNode() ; temp ; temp=tree.nextNode(temp) ) 02088 if( temp->children && temp->d>=_minDepth ) 02089 { 02090 int hasNormals=0; 02091 for( int i=0 ; i<Cube::CORNERS && !hasNormals ; i++ ) hasNormals = HasNormals( &temp->children[i] , EPSILON/(1<<maxDepth) ); 02092 if( !hasNormals ) temp->children=NULL; 02093 } 02094 MemoryUsage(); 02095 } 02096 template<int Degree> 02097 void Octree<Degree>::SetLaplacianConstraints( void ) 02098 { 02099 // To set the Laplacian constraints, we iterate over the 02100 // splatted normals and compute the dot-product of the 02101 // divergence of the normal field with all the basis functions. 02102 // Within the same depth: set directly as a gather 02103 // Coarser depths 02104 fData.setDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG ); 02105 02106 int maxDepth = _sNodes.maxDepth-1; 02107 Point3D< Real > zeroPoint; 02108 zeroPoint[0] = zeroPoint[1] = zeroPoint[2] = 0; 02109 std::vector< Real > constraints( _sNodes.nodeCount[maxDepth] , Real(0) ); 02110 std::vector< Point3D< Real > > coefficients( _sNodes.nodeCount[maxDepth] , zeroPoint ); 02111 02112 // Clear the constraints 02113 #pragma omp parallel for num_threads( threads ) 02114 for( int i=0 ; i<_sNodes.nodeCount[maxDepth+1] ; i++ ) _sNodes.treeNodes[i]->nodeData.constraint = Real( 0. ); 02115 02116 // For the scattering part of the operation, we parallelize by duplicating the constraints and then summing at the end. 02117 std::vector< std::vector< Real > > _constraints( threads ); 02118 for( int t=0 ; t<threads ; t++ ) _constraints[t].resize( _sNodes.nodeCount[maxDepth] , 0 ); 02119 02120 for( int d=maxDepth ; d>=0 ; d-- ) 02121 { 02122 Point3D< double > stencil[5][5][5]; 02123 SetDivergenceStencil( d , &stencil[0][0][0] , false ); 02124 Stencil< Point3D< double > , 5 > stencils[2][2][2]; 02125 SetDivergenceStencils( d , stencils , true ); 02126 #pragma omp parallel for num_threads( threads ) 02127 for( int t=0 ; t<threads ; t++ ) 02128 { 02129 TreeOctNode::NeighborKey5 neighborKey5; 02130 neighborKey5.set( fData.depth ); 02131 int start = _sNodes.nodeCount[d] , end = _sNodes.nodeCount[d+1] , range = end-start; 02132 for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ ) 02133 { 02134 TreeOctNode* node = _sNodes.treeNodes[i]; 02135 int startX=0 , endX=5 , startY=0 , endY=5 , startZ=0 , endZ=5; 02136 int depth = node->depth(); 02137 neighborKey5.getNeighbors( node ); 02138 02139 bool isInterior , isInterior2; 02140 { 02141 int d , off[3]; 02142 node->depthAndOffset( d , off ); 02143 int mn = 2 , mx = (1<<d)-2; 02144 isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx ); 02145 mn += 2 , mx -= 2; 02146 isInterior2 = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx ); 02147 } 02148 int cx , cy , cz; 02149 if( d ) 02150 { 02151 int c = int( node - node->parent->children ); 02152 Cube::FactorCornerIndex( c , cx , cy , cz ); 02153 } 02154 else cx = cy = cz = 0; 02155 Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz]; 02156 02157 // Set constraints from current depth 02158 { 02159 const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth]; 02160 02161 if( isInterior ) 02162 for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ ) 02163 { 02164 const TreeOctNode* _node = neighbors5.neighbors[x][y][z]; 02165 if( _node && _node->nodeData.normalIndex>=0 ) 02166 { 02167 const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex]; 02168 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] ); 02169 } 02170 } 02171 else 02172 for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ ) 02173 { 02174 const TreeOctNode* _node = neighbors5.neighbors[x][y][z]; 02175 if( _node && _node->nodeData.normalIndex>=0 ) 02176 { 02177 const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex]; 02178 node->nodeData.constraint += GetDivergence( _node , node , _normal ); 02179 } 02180 } 02181 UpdateCoarserSupportBounds( neighbors5.neighbors[2][2][2] , startX , endX , startY , endY , startZ , endZ ); 02182 } 02183 if( node->nodeData.nodeIndex<0 || node->nodeData.normalIndex<0 ) continue; 02184 const Point3D< Real >& normal = (*normals)[node->nodeData.normalIndex]; 02185 if( normal[0]==0 && normal[1]==0 && normal[2]==0 ) continue; 02186 if( depth<maxDepth ) coefficients[i] += normal; 02187 02188 if( depth ) 02189 { 02190 const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth-1]; 02191 02192 for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ ) 02193 if( neighbors5.neighbors[x][y][z] ) 02194 { 02195 TreeOctNode* _node = neighbors5.neighbors[x][y][z]; 02196 if( isInterior2 ) 02197 { 02198 Point3D< double >& div = _stencil.values[x][y][z]; 02199 _constraints[t][ _node->nodeData.nodeIndex ] += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] ); 02200 } 02201 else _constraints[t][ _node->nodeData.nodeIndex ] += GetDivergence( node , _node , normal ); 02202 } 02203 } 02204 } 02205 } 02206 } 02207 #pragma omp parallel for num_threads( threads ) schedule( static ) 02208 for( int i=0 ; i<_sNodes.nodeCount[maxDepth] ; i++ ) 02209 { 02210 Real cSum = Real(0.); 02211 for( int t=0 ; t<threads ; t++ ) cSum += _constraints[t][i]; 02212 constraints[i] = cSum; 02213 } 02214 // Fine-to-coarse down-sampling of constraints 02215 for( int d=maxDepth-1 ; d>=0 ; d-- ) DownSample( d , _sNodes , &constraints[0] ); 02216 02217 // Coarse-to-fine up-sampling of coefficients 02218 for( int d=0 ; d<maxDepth ; d++ ) UpSample( d , _sNodes , &coefficients[0] ); 02219 02220 // Add the accumulated constraints from all finer depths 02221 #pragma omp parallel for num_threads( threads ) 02222 for( int i=0 ; i<_sNodes.nodeCount[maxDepth] ; i++ ) _sNodes.treeNodes[i]->nodeData.constraint += constraints[i]; 02223 02224 // Compute the contribution from all coarser depths 02225 for( int d=0 ; d<=maxDepth ; d++ ) 02226 { 02227 int start = _sNodes.nodeCount[d] , end = _sNodes.nodeCount[d+1] , range = end - start; 02228 Stencil< Point3D< double > , 5 > stencils[2][2][2]; 02229 SetDivergenceStencils( d , stencils , false ); 02230 #pragma omp parallel for num_threads( threads ) 02231 for( int t=0 ; t<threads ; t++ ) 02232 { 02233 TreeOctNode::NeighborKey5 neighborKey5; 02234 neighborKey5.set( maxDepth ); 02235 for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ ) 02236 { 02237 TreeOctNode* node = _sNodes.treeNodes[i]; 02238 int depth = node->depth(); 02239 if( !depth ) continue; 02240 int startX=0 , endX=5 , startY=0 , endY=5 , startZ=0 , endZ=5; 02241 UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ ); 02242 const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.getNeighbors( node->parent ); 02243 02244 bool isInterior; 02245 { 02246 int d , off[3]; 02247 node->depthAndOffset( d , off ); 02248 int mn = 4 , mx = (1<<d)-4; 02249 isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx ); 02250 } 02251 int cx , cy , cz; 02252 if( d ) 02253 { 02254 int c = int( node - node->parent->children ); 02255 Cube::FactorCornerIndex( c , cx , cy , cz ); 02256 } 02257 else cx = cy = cz = 0; 02258 Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz]; 02259 02260 Real constraint = Real(0); 02261 for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ ) 02262 if( neighbors5.neighbors[x][y][z] ) 02263 { 02264 TreeOctNode* _node = neighbors5.neighbors[x][y][z]; 02265 int _i = _node->nodeData.nodeIndex; 02266 if( isInterior ) 02267 { 02268 Point3D< double >& div = _stencil.values[x][y][z]; 02269 Point3D< Real >& normal = coefficients[_i]; 02270 constraint += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] ); 02271 } 02272 else constraint += GetDivergence( _node , node , coefficients[_i] ); 02273 } 02274 node->nodeData.constraint += constraint; 02275 } 02276 } 02277 } 02278 02279 fData.clearDotTables( fData.DV_DOT_FLAG ); 02280 02281 // Set the point weights for evaluating the iso-value 02282 #pragma omp parallel for num_threads( threads ) 02283 for( int t=0 ; t<threads ; t++ ) 02284 for( int i=(_sNodes.nodeCount[maxDepth+1]*t)/threads ; i<(_sNodes.nodeCount[maxDepth+1]*(t+1))/threads ; i++ ) 02285 { 02286 TreeOctNode* temp = _sNodes.treeNodes[i]; 02287 if( temp->nodeData.nodeIndex<0 || temp->nodeData.normalIndex<0 ) temp->nodeData.centerWeightContribution = 0; 02288 else temp->nodeData.centerWeightContribution = Real( Length((*normals)[temp->nodeData.normalIndex]) ); 02289 } 02290 MemoryUsage(); 02291 delete normals; 02292 normals = NULL; 02293 } 02294 template<int Degree> 02295 void Octree<Degree>::AdjacencyCountFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencyCount++;} 02296 template<int Degree> 02297 void Octree<Degree>::AdjacencySetFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencies[adjacencyCount++]=node1->nodeData.nodeIndex;} 02298 template<int Degree> 02299 void Octree<Degree>::RefineFunction::Function( TreeOctNode* node1 , const TreeOctNode* node2 ) 02300 { 02301 if( !node1->children && node1->depth()<depth ) node1->initChildren(); 02302 } 02303 template< int Degree > 02304 void Octree< Degree >::FaceEdgesFunction::Function( const TreeOctNode* node1 , const TreeOctNode* node2 ) 02305 { 02306 if( !node1->children && MarchingCubes::HasRoots( node1->nodeData.mcIndex ) ) 02307 { 02308 RootInfo ri1 , ri2; 02309 hash_map< long long , std::pair< RootInfo , int > >::iterator iter; 02310 int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES]; 02311 int count=MarchingCubes::AddTriangleIndices( node1->nodeData.mcIndex , isoTri ); 02312 02313 for( int j=0 ; j<count ; j++ ) 02314 for( int k=0 ; k<3 ; k++ ) 02315 if( fIndex==Cube::FaceAdjacentToEdges( isoTri[j*3+k] , isoTri[j*3+((k+1)%3)] ) ) 02316 if( GetRootIndex( node1 , isoTri[j*3+k] , maxDepth , ri1 ) && GetRootIndex( node1 , isoTri[j*3+((k+1)%3)] , maxDepth , ri2 ) ) 02317 { 02318 long long key1=ri1.key , key2=ri2.key; 02319 edges->push_back( std::pair< RootInfo , RootInfo >( ri2 , ri1 ) ); 02320 iter = vertexCount->find( key1 ); 02321 if( iter==vertexCount->end() ) 02322 { 02323 (*vertexCount)[key1].first = ri1; 02324 (*vertexCount)[key1].second=0; 02325 } 02326 iter=vertexCount->find(key2); 02327 if( iter==vertexCount->end() ) 02328 { 02329 (*vertexCount)[key2].first = ri2; 02330 (*vertexCount)[key2].second=0; 02331 } 02332 (*vertexCount)[key1].second--; 02333 (*vertexCount)[key2].second++; 02334 } 02335 else fprintf( stderr , "Bad Edge 1: %d %d\n" , ri1.key , ri2.key ); 02336 } 02337 } 02338 02339 template< int Degree > 02340 void Octree< Degree >::RefineBoundary( int subdivideDepth ) 02341 { 02342 // This implementation is somewhat tricky. 02343 // We would like to ensure that leaf-nodes across a subdivision boundary have the same depth. 02344 // We do this by calling the setNeighbors function. 02345 // The key is to implement this in a single pass through the leaves, ensuring that refinements don't propogate. 02346 // To this end, we do the minimal refinement that ensures that a cross boundary neighbor, and any of its cross-boundary 02347 // neighbors are all refined simultaneously. 02348 // For this reason, the implementation can only support nodes deeper than sDepth. 02349 bool flags[3][3][3]; 02350 int maxDepth = tree.maxDepth(); 02351 02352 int sDepth; 02353 if( subdivideDepth<=0 ) sDepth = 0; 02354 else sDepth = maxDepth-subdivideDepth; 02355 if( sDepth<=0 ) return; 02356 02357 // Ensure that face adjacent neighbors across the subdivision boundary exist to allow for 02358 // a consistent definition of the iso-surface 02359 TreeOctNode::NeighborKey3 nKey; 02360 nKey.set( maxDepth ); 02361 for( TreeOctNode* leaf=tree.nextLeaf() ; leaf ; leaf=tree.nextLeaf( leaf ) ) 02362 if( leaf->depth()>sDepth ) 02363 { 02364 int d , off[3] , _off[3]; 02365 leaf->depthAndOffset( d , off ); 02366 int res = (1<<d)-1 , _res = ( 1<<(d-sDepth) )-1; 02367 _off[0] = off[0]&_res , _off[1] = off[1]&_res , _off[2] = off[2]&_res; 02368 bool boundary[3][2] = 02369 { 02370 { (off[0]!=0 && _off[0]==0) , (off[0]!=res && _off[0]==_res) } , 02371 { (off[1]!=0 && _off[1]==0) , (off[1]!=res && _off[1]==_res) } , 02372 { (off[2]!=0 && _off[2]==0) , (off[2]!=res && _off[2]==_res) } 02373 }; 02374 02375 if( boundary[0][0] || boundary[0][1] || boundary[1][0] || boundary[1][1] || boundary[2][0] || boundary[2][1] ) 02376 { 02377 TreeOctNode::Neighbors3& neighbors = nKey.getNeighbors( leaf ); 02378 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; 02379 int x=0 , y=0 , z=0; 02380 if ( boundary[0][0] && !neighbors.neighbors[0][1][1] ) x = -1; 02381 else if( boundary[0][1] && !neighbors.neighbors[2][1][1] ) x = 1; 02382 if ( boundary[1][0] && !neighbors.neighbors[1][0][1] ) y = -1; 02383 else if( boundary[1][1] && !neighbors.neighbors[1][2][1] ) y = 1; 02384 if ( boundary[2][0] && !neighbors.neighbors[1][1][0] ) z = -1; 02385 else if( boundary[2][1] && !neighbors.neighbors[1][1][2] ) z = 1; 02386 02387 if( x || y || z ) 02388 { 02389 // Corner case 02390 if( x && y && z ) flags[1+x][1+y][1+z] = true; 02391 // Edge cases 02392 if( x && y ) flags[1+x][1+y][1 ] = true; 02393 if( x && z ) flags[1+x][1 ][1+z] = true; 02394 if( y && z ) flags[1 ][1+y][1+1] = true; 02395 // Face cases 02396 if( x ) flags[1+x][1 ][1 ] = true; 02397 if( y ) flags[1 ][1+y][1 ] = true; 02398 if( z ) flags[1 ][1 ][1+z] = true; 02399 nKey.setNeighbors( leaf , flags ); 02400 } 02401 } 02402 } 02403 _sNodes.set( tree ); 02404 MemoryUsage(); 02405 } 02406 template<int Degree> 02407 void Octree<Degree>::GetMCIsoTriangles( Real isoValue , int subdivideDepth , CoredMeshData* mesh , int fullDepthIso , int nonLinearFit , bool addBarycenter , bool polygonMesh ) 02408 { 02409 fData.setValueTables( fData.VALUE_FLAG | fData.D_VALUE_FLAG , 0 , postNormalSmooth ); 02410 02411 // Ensure that the subtrees are self-contained 02412 RefineBoundary( subdivideDepth ); 02413 02414 RootData rootData , coarseRootData; 02415 std::vector< Point3D< float > >* interiorPoints; 02416 int maxDepth = tree.maxDepth(); 02417 02418 int sDepth = subdivideDepth<=0 ? 0 : std::max< int >( 0 , maxDepth-subdivideDepth ); 02419 02420 std::vector< Real > metSolution( _sNodes.nodeCount[maxDepth] , 0 ); 02421 #pragma omp parallel for num_threads( threads ) 02422 for( int i=_sNodes.nodeCount[_minDepth] ; i<_sNodes.nodeCount[maxDepth] ; i++ ) metSolution[i] = _sNodes.treeNodes[i]->nodeData.solution; 02423 for( int d=0 ; d<maxDepth ; d++ ) UpSample( d , _sNodes , &metSolution[0] ); 02424 02425 // Clear the marching cube indices 02426 #pragma omp parallel for num_threads( threads ) 02427 for( int i=0 ; i<_sNodes.nodeCount[maxDepth+1] ; i++ ) _sNodes.treeNodes[i]->nodeData.mcIndex = 0; 02428 02429 rootData.boundaryValues = new hash_map< long long , std::pair< Real , Point3D< Real > > >(); 02430 int offSet = 0; 02431 02432 int maxCCount = _sNodes.getMaxCornerCount( &tree , sDepth , maxDepth , threads ); 02433 int maxECount = _sNodes.getMaxEdgeCount ( &tree , sDepth , threads ); 02434 rootData.cornerValues = new Real [ maxCCount ]; 02435 rootData.cornerNormals = new Point3D< Real >[ maxCCount ]; 02436 rootData.interiorRoots = new int [ maxECount ]; 02437 rootData.cornerValuesSet = new char[ maxCCount ]; 02438 rootData.cornerNormalsSet = new char[ maxCCount ]; 02439 rootData.edgesSet = new char[ maxECount ]; 02440 _sNodes.setCornerTable( coarseRootData , &tree , sDepth , threads ); 02441 coarseRootData.cornerValues = new Real[ coarseRootData.cCount ]; 02442 coarseRootData.cornerNormals = new Point3D< Real >[ coarseRootData.cCount ]; 02443 coarseRootData.cornerValuesSet = new char[ coarseRootData.cCount ]; 02444 coarseRootData.cornerNormalsSet = new char[ coarseRootData.cCount ]; 02445 memset( coarseRootData.cornerValuesSet , 0 , sizeof( char ) * coarseRootData.cCount ); 02446 memset( coarseRootData.cornerNormalsSet , 0 , sizeof( char ) * coarseRootData.cCount ); 02447 MemoryUsage(); 02448 02449 std::vector< TreeOctNode::ConstNeighborKey3 > nKeys( threads ); 02450 for( int t=0 ; t<threads ; t++ ) nKeys[t].set( maxDepth ); 02451 TreeOctNode::ConstNeighborKey3 nKey; 02452 std::vector< TreeOctNode::ConstNeighborKey5 > nKeys5( threads ); 02453 for( int t=0 ; t<threads ; t++ ) nKeys5[t].set( maxDepth ); 02454 TreeOctNode::ConstNeighborKey5 nKey5; 02455 nKey5.set( maxDepth ); 02456 nKey.set( maxDepth ); 02457 // First process all leaf nodes at depths strictly finer than sDepth, one subtree at a time. 02458 for( int i=_sNodes.nodeCount[sDepth] ; i<_sNodes.nodeCount[sDepth+1] ; i++ ) 02459 { 02460 if( !_sNodes.treeNodes[i]->children ) continue; 02461 02462 _sNodes.setCornerTable( rootData , _sNodes.treeNodes[i] , threads ); 02463 _sNodes.setEdgeTable ( rootData , _sNodes.treeNodes[i] , threads ); 02464 memset( rootData.cornerValuesSet , 0 , sizeof( char ) * rootData.cCount ); 02465 memset( rootData.cornerNormalsSet , 0 , sizeof( char ) * rootData.cCount ); 02466 memset( rootData.edgesSet , 0 , sizeof( char ) * rootData.eCount ); 02467 interiorPoints = new std::vector< Point3D< float > >(); 02468 for( int d=maxDepth ; d>sDepth ; d-- ) 02469 { 02470 int leafNodeCount = 0; 02471 std::vector< TreeOctNode* > leafNodes; 02472 for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodeCount++; 02473 leafNodes.reserve( leafNodeCount ); 02474 for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodes.push_back( node ); 02475 Stencil< double , 3 > stencil1[8] , stencil2[8][8]; 02476 SetEvaluationStencils( d , stencil1 , stencil2 ); 02477 02478 // First set the corner values and associated marching-cube indices 02479 #pragma omp parallel for num_threads( threads ) 02480 for( int t=0 ; t<threads ; t++ ) for( int i=(leafNodeCount*t)/threads ; i<(leafNodeCount*(t+1))/threads ; i++ ) 02481 { 02482 TreeOctNode* leaf = leafNodes[i]; 02483 SetIsoCorners( isoValue , leaf , rootData , rootData.cornerValuesSet , rootData.cornerValues , nKeys[t] , &metSolution[0] , stencil1 , stencil2 ); 02484 02485 // If this node shares a vertex with a coarser node, set the vertex value 02486 int d , off[3]; 02487 leaf->depthAndOffset( d , off ); 02488 int res = 1<<(d-sDepth); 02489 off[0] %= res , off[1] %=res , off[2] %= res; 02490 res--; 02491 if( !(off[0]%res) && !(off[1]%res) && !(off[2]%res) ) 02492 { 02493 const TreeOctNode* temp = leaf; 02494 while( temp->d!=sDepth ) temp = temp->parent; 02495 int x = off[0]==0 ? 0 : 1 , y = off[1]==0 ? 0 : 1 , z = off[2]==0 ? 0 : 1; 02496 int c = Cube::CornerIndex( x , y , z ); 02497 int idx = coarseRootData.cornerIndices( temp )[ c ]; 02498 coarseRootData.cornerValues[ idx ] = rootData.cornerValues[ rootData.cornerIndices( leaf )[c] ]; 02499 coarseRootData.cornerValuesSet[ idx ] = true; 02500 } 02501 02502 // Compute the iso-vertices 02503 if( MarchingCubes::HasRoots( leaf->nodeData.mcIndex ) ) 02504 SetMCRootPositions( leaf , sDepth , isoValue , nKeys5[t] , rootData , interiorPoints , mesh , &metSolution[0] , nonLinearFit ); 02505 } 02506 // Note that this should be broken off for multi-threading as 02507 // the SetMCRootPositions writes to interiorPoints (with lockupdateing) 02508 // while GetMCIsoTriangles reads from interiorPoints (without locking) 02509 #if MISHA_DEBUG 02510 std::vector< Point3D< float > > barycenters; 02511 std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? & barycenters : NULL; 02512 #endif // MISHA_DEBUG 02513 #pragma omp parallel for num_threads( threads ) 02514 for( int t=0 ; t<threads ; t++ ) for( int i=(leafNodeCount*t)/threads ; i<(leafNodeCount*(t+1))/threads ; i++ ) 02515 { 02516 TreeOctNode* leaf = leafNodes[i]; 02517 if( MarchingCubes::HasRoots( leaf->nodeData.mcIndex ) ) 02518 #if MISHA_DEBUG 02519 GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , polygonMesh , barycenterPtr ); 02520 #else // !MISHA_DEBUG 02521 GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , addBarycenter , polygonMesh ); 02522 #endif // MISHA_DEBUG 02523 } 02524 #if MISHA_DEBUG 02525 for( int i=0 ; i<barycenters.size() ; i++ ) interiorPoints->push_back( barycenters[i] ); 02526 #endif // MISHA_DEBUG 02527 } 02528 offSet = mesh->outOfCorePointCount(); 02529 #if 1 02530 delete interiorPoints; 02531 #endif 02532 } 02533 MemoryUsage(); 02534 delete[] rootData.cornerValues , delete[] rootData.cornerNormals , rootData.cornerValues = NULL , rootData.cornerNormals = NULL; 02535 delete[] rootData.cornerValuesSet , delete[] rootData.cornerNormalsSet , rootData.cornerValuesSet = NULL , rootData.cornerNormalsSet = NULL; 02536 delete[] rootData.interiorRoots ; rootData.interiorRoots = NULL; 02537 delete[] rootData.edgesSet ; rootData.edgesSet = NULL; 02538 coarseRootData.interiorRoots = NULL; 02539 coarseRootData.boundaryValues = rootData.boundaryValues; 02540 for( poisson::hash_map< long long , int >::iterator iter=rootData.boundaryRoots.begin() ; iter!=rootData.boundaryRoots.end() ; iter++ ) 02541 coarseRootData.boundaryRoots[iter->first] = iter->second; 02542 02543 for( int d=sDepth ; d>=0 ; d-- ) 02544 { 02545 Stencil< double , 3 > stencil1[8] , stencil2[8][8]; 02546 SetEvaluationStencils( d , stencil1 , stencil2 ); 02547 #if MISHA_DEBUG 02548 std::vector< Point3D< float > > barycenters; 02549 std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? &barycenters : NULL; 02550 #endif // MISHA_DEBUG 02551 for( int i=_sNodes.nodeCount[d] ; i<_sNodes.nodeCount[d+1] ; i++ ) 02552 { 02553 TreeOctNode* leaf = _sNodes.treeNodes[i]; 02554 if( leaf->children ) continue; 02555 02556 // First set the corner values and associated marching-cube indices 02557 SetIsoCorners( isoValue , leaf , coarseRootData , coarseRootData.cornerValuesSet , coarseRootData.cornerValues , nKey , &metSolution[0] , stencil1 , stencil2 ); 02558 02559 // Now compute the iso-vertices 02560 if( MarchingCubes::HasRoots( leaf->nodeData.mcIndex ) ) 02561 { 02562 SetMCRootPositions( leaf , 0 , isoValue , nKey5 , coarseRootData , NULL , mesh , &metSolution[0] , nonLinearFit ); 02563 #if MISHA_DEBUG 02564 GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , polygonMesh , barycenterPtr ); 02565 #else // !MISHA_DEBUG 02566 GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , addBarycenter , polygonMesh ); 02567 #endif // MISHA_DEBUG 02568 } 02569 } 02570 } 02571 MemoryUsage(); 02572 02573 delete[] coarseRootData.cornerValues , delete[] coarseRootData.cornerNormals; 02574 delete[] coarseRootData.cornerValuesSet , delete[] coarseRootData.cornerNormalsSet; 02575 delete rootData.boundaryValues; 02576 } 02577 template<int Degree> 02578 Real Octree<Degree>::getCenterValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey , const TreeOctNode* node){ 02579 int idx[3]; 02580 Real value=0; 02581 02582 VertexData::CenterIndex(node,fData.depth,idx); 02583 idx[0]*=fData.functionCount; 02584 idx[1]*=fData.functionCount; 02585 idx[2]*=fData.functionCount; 02586 int minDepth = std::max< int >( 0 , std::min< int >( _minDepth , node->depth()-1 ) ); 02587 for( int i=minDepth ; i<=node->depth() ; i++ ) 02588 for(int j=0;j<3;j++) 02589 for(int k=0;k<3;k++) 02590 for(int l=0;l<3;l++) 02591 { 02592 const TreeOctNode* n=neighborKey.neighbors[i].neighbors[j][k][l]; 02593 if( n ) 02594 { 02595 Real temp=n->nodeData.solution; 02596 value+=temp*Real( 02597 fData.valueTables[idx[0]+int(n->off[0])]* 02598 fData.valueTables[idx[1]+int(n->off[1])]* 02599 fData.valueTables[idx[2]+int(n->off[2])]); 02600 } 02601 } 02602 if(node->children) 02603 { 02604 for(int i=0;i<Cube::CORNERS;i++){ 02605 int ii=Cube::AntipodalCornerIndex(i); 02606 const TreeOctNode* n=&node->children[i]; 02607 while(1){ 02608 value+=n->nodeData.solution*Real( 02609 fData.valueTables[idx[0]+int(n->off[0])]* 02610 fData.valueTables[idx[1]+int(n->off[1])]* 02611 fData.valueTables[idx[2]+int(n->off[2])]); 02612 if( n->children ) n=&n->children[ii]; 02613 else break; 02614 } 02615 } 02616 } 02617 return value; 02618 } 02619 template< int Degree > 02620 Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution ) 02621 { 02622 int idx[3]; 02623 double value = 0; 02624 02625 VertexData::CornerIndex( node , corner , fData.depth , idx ); 02626 idx[0] *= fData.functionCount; 02627 idx[1] *= fData.functionCount; 02628 idx[2] *= fData.functionCount; 02629 02630 int d = node->depth(); 02631 int cx , cy , cz; 02632 int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3; 02633 Cube::FactorCornerIndex( corner , cx , cy , cz ); 02634 { 02635 TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d]; 02636 if( cx==0 ) endX = 2; 02637 else startX = 1; 02638 if( cy==0 ) endY = 2; 02639 else startY = 1; 02640 if( cz==0 ) endZ = 2; 02641 else startZ = 1; 02642 for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ ) 02643 { 02644 const TreeOctNode* n=neighbors.neighbors[x][y][z]; 02645 if( n ) 02646 { 02647 double v = 02648 fData.valueTables[ idx[0]+int(n->off[0]) ]* 02649 fData.valueTables[ idx[1]+int(n->off[1]) ]* 02650 fData.valueTables[ idx[2]+int(n->off[2]) ]; 02651 value += n->nodeData.solution * v; 02652 } 02653 } 02654 } 02655 if( d>0 && d>_minDepth ) 02656 { 02657 int _corner = int( node - node->parent->children ); 02658 int _cx , _cy , _cz; 02659 Cube::FactorCornerIndex( _corner , _cx , _cy , _cz ); 02660 if( cx!=_cx ) startX = 0 , endX = 3; 02661 if( cy!=_cy ) startY = 0 , endY = 3; 02662 if( cz!=_cz ) startZ = 0 , endZ = 3; 02663 TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1]; 02664 for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ ) 02665 { 02666 const TreeOctNode* n=neighbors.neighbors[x][y][z]; 02667 if( n ) 02668 { 02669 double v = 02670 fData.valueTables[ idx[0]+int(n->off[0]) ]* 02671 fData.valueTables[ idx[1]+int(n->off[1]) ]* 02672 fData.valueTables[ idx[2]+int(n->off[2]) ]; 02673 value += metSolution[ n->nodeData.nodeIndex ] * v; 02674 } 02675 } 02676 } 02677 return Real( value ); 02678 } 02679 template< int Degree > 02680 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] ) 02681 { 02682 double value = 0; 02683 int d = node->depth(); 02684 int cx , cy , cz; 02685 int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3; 02686 Cube::FactorCornerIndex( corner , cx , cy , cz ); 02687 { 02688 TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d]; 02689 if( cx==0 ) endX = 2; 02690 else startX = 1; 02691 if( cy==0 ) endY = 2; 02692 else startY = 1; 02693 if( cz==0 ) endZ = 2; 02694 else startZ = 1; 02695 for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ ) 02696 { 02697 const TreeOctNode* n=neighbors.neighbors[x][y][z]; 02698 if( n ) value += n->nodeData.solution * stencil1[x][y][z]; 02699 } 02700 } 02701 if( d>0 && d>_minDepth ) 02702 { 02703 int _corner = int( node - node->parent->children ); 02704 int _cx , _cy , _cz; 02705 Cube::FactorCornerIndex( _corner , _cx , _cy , _cz ); 02706 if( cx!=_cx ) startX = 0 , endX = 3; 02707 if( cy!=_cy ) startY = 0 , endY = 3; 02708 if( cz!=_cz ) startZ = 0 , endZ = 3; 02709 TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1]; 02710 for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ ) 02711 { 02712 const TreeOctNode* n=neighbors.neighbors[x][y][z]; 02713 if( n ) value += metSolution[ n->nodeData.nodeIndex ] * stencil2[x][y][z]; 02714 } 02715 } 02716 return Real( value ); 02717 } 02718 template< int Degree > 02719 Point3D< Real > Octree< Degree >::getCornerNormal( const OctNode< TreeNodeData , Real >::ConstNeighborKey5& neighborKey5 , const TreeOctNode* node , int corner , const Real* metSolution ) 02720 { 02721 int idx[3]; 02722 Point3D< Real > normal; 02723 normal[0] = normal[1] = normal[2] = 0.; 02724 02725 VertexData::CornerIndex( node , corner , fData.depth , idx ); 02726 idx[0] *= fData.functionCount; 02727 idx[1] *= fData.functionCount; 02728 idx[2] *= fData.functionCount; 02729 02730 int d = node->depth(); 02731 // Iterate over all ancestors that can overlap the corner 02732 { 02733 TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d]; 02734 for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ ) 02735 { 02736 const TreeOctNode* n=neighbors.neighbors[j][k][l]; 02737 if( n ) 02738 { 02739 int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] }; 02740 double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] }; 02741 double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] }; 02742 Real solution = n->nodeData.solution; 02743 normal[0] += Real( dValues[0] * values[1] * values[2] * solution ); 02744 normal[1] += Real( values[0] * dValues[1] * values[2] * solution ); 02745 normal[2] += Real( values[0] * values[1] * dValues[2] * solution ); 02746 } 02747 } 02748 } 02749 if( d>0 && d>_minDepth ) 02750 { 02751 TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d-1]; 02752 for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ ) 02753 { 02754 const TreeOctNode* n=neighbors.neighbors[j][k][l]; 02755 if( n ) 02756 { 02757 int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] }; 02758 double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] }; 02759 double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] }; 02760 Real solution = metSolution[ n->nodeData.nodeIndex ]; 02761 normal[0] += Real( dValues[0] * values[1] * values[2] * solution ); 02762 normal[1] += Real( values[0] * dValues[1] * values[2] * solution ); 02763 normal[2] += Real( values[0] * values[1] * dValues[2] * solution ); 02764 } 02765 } 02766 } 02767 return normal; 02768 } 02769 template< int Degree > 02770 Real Octree<Degree>::GetIsoValue( void ) 02771 { 02772 Real isoValue , weightSum; 02773 02774 neighborKey2.set( fData.depth ); 02775 fData.setValueTables( fData.VALUE_FLAG , 0 ); 02776 02777 isoValue = weightSum = 0; 02778 #pragma omp parallel for num_threads( threads ) reduction( + : isoValue , weightSum ) 02779 for( int t=0 ; t<threads ; t++) 02780 { 02781 TreeOctNode::ConstNeighborKey3 nKey; 02782 nKey.set( _sNodes.maxDepth-1 ); 02783 int nodeCount = _sNodes.nodeCount[ _sNodes.maxDepth ]; 02784 for( int i=(nodeCount*t)/threads ; i<(nodeCount*(t+1))/threads ; i++ ) 02785 { 02786 TreeOctNode* temp = _sNodes.treeNodes[i]; 02787 nKey.getNeighbors( temp ); 02788 Real w = temp->nodeData.centerWeightContribution; 02789 if( w!=0 ) 02790 { 02791 isoValue += getCenterValue( nKey , temp ) * w; 02792 weightSum += w; 02793 } 02794 } 02795 } 02796 return isoValue/weightSum; 02797 } 02798 02799 template< int Degree > 02800 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] ) 02801 { 02802 Real cornerValues[ Cube::CORNERS ]; 02803 const SortedTreeNodes::CornerIndices& cIndices = cData[ leaf ]; 02804 02805 bool isInterior; 02806 int d , off[3]; 02807 leaf->depthAndOffset( d , off ); 02808 int mn = 2 , mx = (1<<d)-2; 02809 isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx ); 02810 nKey.getNeighbors( leaf ); 02811 for( int c=0 ; c<Cube::CORNERS ; c++ ) 02812 { 02813 int vIndex = cIndices[c]; 02814 if( valuesSet[vIndex] ) cornerValues[c] = values[vIndex]; 02815 else 02816 { 02817 if( isInterior ) cornerValues[c] = getCornerValue( nKey , leaf , c , metSolution , stencil1[c].values , stencil2[int(leaf - leaf->parent->children)][c].values ); 02818 else cornerValues[c] = getCornerValue( nKey , leaf , c , metSolution ); 02819 values[vIndex] = cornerValues[c]; 02820 valuesSet[vIndex] = 1; 02821 } 02822 } 02823 02824 leaf->nodeData.mcIndex = MarchingCubes::GetIndex( cornerValues , isoValue ); 02825 02826 // Set the marching cube indices for all interior nodes. 02827 if( leaf->parent ) 02828 { 02829 TreeOctNode* parent = leaf->parent; 02830 int c = int( leaf - leaf->parent->children ); 02831 int mcid = leaf->nodeData.mcIndex & (1<<MarchingCubes::cornerMap()[c]); 02832 02833 if( mcid ) 02834 { 02835 poisson::atomicOr(parent->nodeData.mcIndex, mcid); 02836 02837 while( 1 ) 02838 { 02839 if( parent->parent && parent->parent->d>=_minDepth && (parent-parent->parent->children)==c ) 02840 { 02841 poisson::atomicOr(parent->parent->nodeData.mcIndex, mcid); 02842 parent = parent->parent; 02843 } 02844 else break; 02845 } 02846 } 02847 } 02848 } 02849 02850 02851 template<int Degree> 02852 int Octree<Degree>::InteriorFaceRootCount(const TreeOctNode* node,const int &faceIndex,int maxDepth){ 02853 int c1,c2,e1,e2,dir,off,cnt=0; 02854 int corners[Cube::CORNERS/2]; 02855 if(node->children){ 02856 Cube::FaceCorners(faceIndex,corners[0],corners[1],corners[2],corners[3]); 02857 Cube::FactorFaceIndex(faceIndex,dir,off); 02858 c1=corners[0]; 02859 c2=corners[3]; 02860 switch(dir){ 02861 case 0: 02862 e1=Cube::EdgeIndex(1,off,1); 02863 e2=Cube::EdgeIndex(2,off,1); 02864 break; 02865 case 1: 02866 e1=Cube::EdgeIndex(0,off,1); 02867 e2=Cube::EdgeIndex(2,1,off); 02868 break; 02869 case 2: 02870 e1=Cube::EdgeIndex(0,1,off); 02871 e2=Cube::EdgeIndex(1,1,off); 02872 break; 02873 }; 02874 cnt+=EdgeRootCount(&node->children[c1],e1,maxDepth)+EdgeRootCount(&node->children[c1],e2,maxDepth); 02875 switch(dir){ 02876 case 0: 02877 e1=Cube::EdgeIndex(1,off,0); 02878 e2=Cube::EdgeIndex(2,off,0); 02879 break; 02880 case 1: 02881 e1=Cube::EdgeIndex(0,off,0); 02882 e2=Cube::EdgeIndex(2,0,off); 02883 break; 02884 case 2: 02885 e1=Cube::EdgeIndex(0,0,off); 02886 e2=Cube::EdgeIndex(1,0,off); 02887 break; 02888 }; 02889 cnt+=EdgeRootCount(&node->children[c2],e1,maxDepth)+EdgeRootCount(&node->children[c2],e2,maxDepth); 02890 for(int i=0;i<Cube::CORNERS/2;i++){if(node->children[corners[i]].children){cnt+=InteriorFaceRootCount(&node->children[corners[i]],faceIndex,maxDepth);}} 02891 } 02892 return cnt; 02893 } 02894 02895 template<int Degree> 02896 int Octree<Degree>::EdgeRootCount(const TreeOctNode* node,int edgeIndex,int maxDepth){ 02897 int f1,f2,c1,c2; 02898 const TreeOctNode* temp; 02899 Cube::FacesAdjacentToEdge(edgeIndex,f1,f2); 02900 02901 int eIndex; 02902 const TreeOctNode* finest=node; 02903 eIndex=edgeIndex; 02904 if(node->depth()<maxDepth){ 02905 temp=node->faceNeighbor(f1); 02906 if(temp && temp->children){ 02907 finest=temp; 02908 eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1); 02909 } 02910 else{ 02911 temp=node->faceNeighbor(f2); 02912 if(temp && temp->children){ 02913 finest=temp; 02914 eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2); 02915 } 02916 else{ 02917 temp=node->edgeNeighbor(edgeIndex); 02918 if(temp && temp->children){ 02919 finest=temp; 02920 eIndex=Cube::EdgeReflectEdgeIndex(edgeIndex); 02921 } 02922 } 02923 } 02924 } 02925 02926 Cube::EdgeCorners(eIndex,c1,c2); 02927 if(finest->children) return EdgeRootCount(&finest->children[c1],eIndex,maxDepth)+EdgeRootCount(&finest->children[c2],eIndex,maxDepth); 02928 else return MarchingCubes::HasEdgeRoots(finest->nodeData.mcIndex,eIndex); 02929 } 02930 template<int Degree> 02931 int Octree<Degree>::IsBoundaryFace(const TreeOctNode* node,int faceIndex,int subdivideDepth){ 02932 int dir,offset,d,o[3],idx; 02933 02934 if(subdivideDepth<0){return 0;} 02935 if(node->d<=subdivideDepth){return 1;} 02936 Cube::FactorFaceIndex(faceIndex,dir,offset); 02937 node->depthAndOffset(d,o); 02938 02939 idx=(int(o[dir])<<1) + (offset<<1); 02940 return !(idx%(2<<(int(node->d)-subdivideDepth))); 02941 } 02942 template<int Degree> 02943 int Octree<Degree>::IsBoundaryEdge(const TreeOctNode* node,int edgeIndex,int subdivideDepth){ 02944 int dir,x,y; 02945 Cube::FactorEdgeIndex(edgeIndex,dir,x,y); 02946 return IsBoundaryEdge(node,dir,x,y,subdivideDepth); 02947 } 02948 template<int Degree> 02949 int Octree<Degree>::IsBoundaryEdge( const TreeOctNode* node , int dir , int x , int y , int subdivideDepth ) 02950 { 02951 int d , o[3] , idx1 , idx2 , mask; 02952 02953 if( subdivideDepth<0 ) return 0; 02954 if( node->d<=subdivideDepth ) return 1; 02955 node->depthAndOffset( d , o ); 02956 02957 switch( dir ) 02958 { 02959 case 0: 02960 idx1 = o[1] + x; 02961 idx2 = o[2] + y; 02962 break; 02963 case 1: 02964 idx1 = o[0] + x; 02965 idx2 = o[2] + y; 02966 break; 02967 case 2: 02968 idx1 = o[0] + x; 02969 idx2 = o[1] + y; 02970 break; 02971 } 02972 mask = 1<<( int(node->d) - subdivideDepth ); 02973 return !(idx1%(mask)) || !(idx2%(mask)); 02974 } 02975 template< int Degree > 02976 void Octree< Degree >::GetRootSpan( const RootInfo& ri , Point3D< float >& start , Point3D< float >& end ) 02977 { 02978 int o , i1 , i2; 02979 Real width; 02980 Point3D< Real > c; 02981 02982 Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 ); 02983 ri.node->centerAndWidth( c , width ); 02984 switch(o) 02985 { 02986 case 0: 02987 start[0] = c[0] - width/2; 02988 end [0] = c[0] + width/2; 02989 start[1] = end[1] = c[1] - width/2 + width*i1; 02990 start[2] = end[2] = c[2] - width/2 + width*i2; 02991 break; 02992 case 1: 02993 start[0] = end[0] = c[0] - width/2 + width*i1; 02994 start[1] = c[1] - width/2; 02995 end [1] = c[1] + width/2; 02996 start[2] = end[2] = c[2] - width/2 + width*i2; 02997 break; 02998 case 2: 02999 start[0] = end[0] = c[0] - width/2 + width*i1; 03000 start[1] = end[1] = c[1] - width/2 + width*i2; 03001 start[2] = c[2] - width/2; 03002 end [2] = c[2] + width/2; 03003 break; 03004 } 03005 } 03006 ////////////////////////////////////////////////////////////////////////////////////// 03007 // The assumption made when calling this code is that the edge has at most one root // 03008 ////////////////////////////////////////////////////////////////////////////////////// 03009 template< int Degree > 03010 int Octree< Degree >::GetRoot( const RootInfo& ri , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , Point3D< Real > & position , RootData& rootData , int sDepth , const Real* metSolution , int nonLinearFit ) 03011 { 03012 if( !MarchingCubes::HasRoots( ri.node->nodeData.mcIndex ) ) return 0; 03013 int c1 , c2; 03014 Cube::EdgeCorners( ri.edgeIndex , c1 , c2 ); 03015 if( !MarchingCubes::HasEdgeRoots( ri.node->nodeData.mcIndex , ri.edgeIndex ) ) return 0; 03016 03017 long long key1 , key2; 03018 Point3D< Real > n[2]; 03019 03020 int i , o , i1 , i2 , rCount=0; 03021 Polynomial<2> P; 03022 std::vector< double > roots; 03023 double x0 , x1; 03024 Real center , width; 03025 Real averageRoot=0; 03026 Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 ); 03027 int idx1[3] , idx2[3]; 03028 key1 = VertexData::CornerIndex( ri.node , c1 , fData.depth , idx1 ); 03029 key2 = VertexData::CornerIndex( ri.node , c2 , fData.depth , idx2 ); 03030 03031 bool isBoundary = ( IsBoundaryEdge( ri.node , ri.edgeIndex , sDepth )!=0 ); 03032 bool haveKey1 , haveKey2; 03033 std::pair< Real , Point3D< Real > > keyValue1 , keyValue2; 03034 int iter1 , iter2; 03035 { 03036 iter1 = rootData.cornerIndices( ri.node )[ c1 ]; 03037 iter2 = rootData.cornerIndices( ri.node )[ c2 ]; 03038 keyValue1.first = rootData.cornerValues[iter1]; 03039 keyValue2.first = rootData.cornerValues[iter2]; 03040 if( isBoundary ) 03041 { 03042 #pragma omp critical (normal_hash_access) 03043 { 03044 haveKey1 = ( rootData.boundaryValues->find( key1 )!=rootData.boundaryValues->end() ); 03045 haveKey2 = ( rootData.boundaryValues->find( key2 )!=rootData.boundaryValues->end() ); 03046 if( haveKey1 ) keyValue1 = (*rootData.boundaryValues)[key1]; 03047 if( haveKey2 ) keyValue2 = (*rootData.boundaryValues)[key2]; 03048 } 03049 } 03050 else 03051 { 03052 haveKey1 = ( rootData.cornerNormalsSet[ iter1 ] != 0 ); 03053 haveKey2 = ( rootData.cornerNormalsSet[ iter2 ] != 0 ); 03054 keyValue1.first = rootData.cornerValues[iter1]; 03055 keyValue2.first = rootData.cornerValues[iter2]; 03056 if( haveKey1 ) keyValue1.second = rootData.cornerNormals[iter1]; 03057 if( haveKey2 ) keyValue2.second = rootData.cornerNormals[iter2]; 03058 } 03059 } 03060 if( !haveKey1 || !haveKey2 ) neighborKey5.getNeighbors( ri.node ); 03061 if( !haveKey1 ) keyValue1.second = getCornerNormal( neighborKey5 , ri.node , c1 , metSolution ); 03062 x0 = keyValue1.first; 03063 n[0] = keyValue1.second; 03064 03065 if( !haveKey2 ) keyValue2.second = getCornerNormal( neighborKey5 , ri.node , c2 , metSolution ); 03066 x1 = keyValue2.first; 03067 n[1] = keyValue2.second; 03068 03069 if( !haveKey1 || !haveKey2 ) 03070 { 03071 if( isBoundary ) 03072 { 03073 #pragma omp critical (normal_hash_access) 03074 { 03075 if( !haveKey1 ) (*rootData.boundaryValues)[key1] = keyValue1; 03076 if( !haveKey2 ) (*rootData.boundaryValues)[key2] = keyValue2; 03077 } 03078 } 03079 else 03080 { 03081 if( !haveKey1 ) rootData.cornerNormals[iter1] = keyValue1.second , rootData.cornerNormalsSet[ iter1 ] = 1; 03082 if( !haveKey2 ) rootData.cornerNormals[iter2] = keyValue2.second , rootData.cornerNormalsSet[ iter2 ] = 1; 03083 } 03084 } 03085 03086 Point3D< Real > c; 03087 ri.node->centerAndWidth(c,width); 03088 center=c[o]; 03089 for( i=0 ; i<DIMENSION ; i++ ) n[0][i] *= width , n[1][i] *= width; 03090 03091 switch(o) 03092 { 03093 case 0: 03094 position[1] = c[1]-width/2+width*i1; 03095 position[2] = c[2]-width/2+width*i2; 03096 break; 03097 case 1: 03098 position[0] = c[0]-width/2+width*i1; 03099 position[2] = c[2]-width/2+width*i2; 03100 break; 03101 case 2: 03102 position[0] = c[0]-width/2+width*i1; 03103 position[1] = c[1]-width/2+width*i2; 03104 break; 03105 } 03106 double dx0,dx1; 03107 dx0 = n[0][o]; 03108 dx1 = n[1][o]; 03109 03110 // The scaling will turn the Hermite Spline into a quadratic 03111 double scl=(x1-x0)/((dx1+dx0)/2); 03112 dx0 *= scl; 03113 dx1 *= scl; 03114 03115 // Hermite Spline 03116 P.coefficients[0] = x0; 03117 P.coefficients[1] = dx0; 03118 P.coefficients[2] = 3*(x1-x0)-dx1-2*dx0; 03119 03120 P.getSolutions( isoValue , roots , EPSILON ); 03121 for( i=0 ; i<int(roots.size()) ; i++ ) 03122 if( roots[i]>=0 && roots[i]<=1 ) 03123 { 03124 averageRoot += Real( roots[i] ); 03125 rCount++; 03126 } 03127 if( rCount && nonLinearFit ) averageRoot /= rCount; 03128 else averageRoot = Real((x0-isoValue)/(x0-x1)); 03129 if( averageRoot<0 || averageRoot>1 ) 03130 { 03131 fprintf( stderr , "[WARNING] Bad average root: %f\n" , averageRoot ); 03132 fprintf( stderr , "\t(%f %f) , (%f %f) (%f)\n" , x0 , x1 , dx0 , dx1 , isoValue ); 03133 if( averageRoot<0 ) averageRoot = 0; 03134 if( averageRoot>1 ) averageRoot = 1; 03135 } 03136 position[o] = Real(center-width/2+width*averageRoot); 03137 return 1; 03138 } 03139 template< int Degree > 03140 int Octree< Degree >::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , int sDepth,RootInfo& ri ) 03141 { 03142 int c1,c2,f1,f2; 03143 const TreeOctNode *temp,*finest; 03144 int finestIndex; 03145 03146 Cube::FacesAdjacentToEdge(edgeIndex,f1,f2); 03147 03148 finest=node; 03149 finestIndex=edgeIndex; 03150 if(node->depth()<maxDepth){ 03151 if(IsBoundaryFace(node,f1,sDepth)){temp=NULL;} 03152 else{temp=node->faceNeighbor(f1);} 03153 if(temp && temp->children){ 03154 finest=temp; 03155 finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1); 03156 } 03157 else{ 03158 if(IsBoundaryFace(node,f2,sDepth)){temp=NULL;} 03159 else{temp=node->faceNeighbor(f2);} 03160 if(temp && temp->children){ 03161 finest=temp; 03162 finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2); 03163 } 03164 else{ 03165 if(IsBoundaryEdge(node,edgeIndex,sDepth)){temp=NULL;} 03166 else{temp=node->edgeNeighbor(edgeIndex);} 03167 if(temp && temp->children){ 03168 finest=temp; 03169 finestIndex=Cube::EdgeReflectEdgeIndex(edgeIndex); 03170 } 03171 } 03172 } 03173 } 03174 03175 Cube::EdgeCorners(finestIndex,c1,c2); 03176 if(finest->children){ 03177 if (GetRootIndex(&finest->children[c1],finestIndex,maxDepth,sDepth,ri)) {return 1;} 03178 else if (GetRootIndex(&finest->children[c2],finestIndex,maxDepth,sDepth,ri)) {return 1;} 03179 else 03180 { 03181 fprintf( stderr , "[WARNING] Couldn't find root index with either child\n" ); 03182 return 0; 03183 } 03184 } 03185 else 03186 { 03187 if( !(MarchingCubes::edgeMask()[finest->nodeData.mcIndex] & (1<<finestIndex)) ) 03188 { 03189 fprintf( stderr , "[WARNING] Finest node does not have iso-edge\n" ); 03190 return 0; 03191 } 03192 03193 int o,i1,i2; 03194 Cube::FactorEdgeIndex(finestIndex,o,i1,i2); 03195 int d,off[3]; 03196 finest->depthAndOffset(d,off); 03197 ri.node=finest; 03198 ri.edgeIndex=finestIndex; 03199 int eIndex[2],offset; 03200 offset=BinaryNode<Real>::Index( d , off[o] ); 03201 switch(o) 03202 { 03203 case 0: 03204 eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1); 03205 eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2); 03206 break; 03207 case 1: 03208 eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1); 03209 eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2); 03210 break; 03211 case 2: 03212 eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1); 03213 eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2); 03214 break; 03215 } 03216 ri.key = (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45; 03217 return 1; 03218 } 03219 } 03220 template<int Degree> 03221 int Octree<Degree>::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , RootInfo& ri ) 03222 { 03223 int c1,c2,f1,f2; 03224 const TreeOctNode *temp,*finest; 03225 int finestIndex; 03226 03227 03228 // The assumption is that the super-edge has a root along it. 03229 if(!(MarchingCubes::edgeMask()[node->nodeData.mcIndex] & (1<<edgeIndex))){return 0;} 03230 03231 Cube::FacesAdjacentToEdge(edgeIndex,f1,f2); 03232 03233 finest = node; 03234 finestIndex = edgeIndex; 03235 if( node->depth()<maxDepth && !node->children ) 03236 { 03237 temp=node->faceNeighbor( f1 ); 03238 if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f1 ); 03239 else 03240 { 03241 temp = node->faceNeighbor( f2 ); 03242 if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f2 ); 03243 else 03244 { 03245 temp = node->edgeNeighbor( edgeIndex ); 03246 if( temp && temp->children ) finest = temp , finestIndex = Cube::EdgeReflectEdgeIndex( edgeIndex ); 03247 } 03248 } 03249 } 03250 03251 Cube::EdgeCorners( finestIndex , c1 , c2 ); 03252 if( finest->children ) 03253 { 03254 if ( GetRootIndex( finest->children + c1 , finestIndex , maxDepth , ri ) ) return 1; 03255 else if( GetRootIndex( finest->children + c2 , finestIndex , maxDepth , ri ) ) return 1; 03256 else 03257 { 03258 int d1 , off1[3] , d2 , off2[3]; 03259 node->depthAndOffset( d1 , off1 ); 03260 finest->depthAndOffset( d2 , off2 ); 03261 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 ); 03262 printf( "\t" ); 03263 for( int i=0 ; i<8 ; i++ ) if( node->nodeData.mcIndex & (1<<i) ) printf( "1" ); else printf( "0" ); 03264 printf( "\t" ); 03265 for( int i=0 ; i<8 ; i++ ) if( finest->nodeData.mcIndex & (1<<i) ) printf( "1" ); else printf( "0" ); 03266 printf( "\n" ); 03267 return 0; 03268 } 03269 } 03270 else 03271 { 03272 int o,i1,i2; 03273 Cube::FactorEdgeIndex(finestIndex,o,i1,i2); 03274 int d,off[3]; 03275 finest->depthAndOffset(d,off); 03276 ri.node=finest; 03277 ri.edgeIndex=finestIndex; 03278 int offset,eIndex[2]; 03279 offset = BinaryNode< Real >::CenterIndex( d , off[o] ); 03280 switch(o){ 03281 case 0: 03282 eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1); 03283 eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2); 03284 break; 03285 case 1: 03286 eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1); 03287 eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2); 03288 break; 03289 case 2: 03290 eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1); 03291 eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2); 03292 break; 03293 } 03294 ri.key= (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45; 03295 return 1; 03296 } 03297 } 03298 template<int Degree> 03299 int Octree<Degree>::GetRootPair(const RootInfo& ri,int maxDepth,RootInfo& pair){ 03300 const TreeOctNode* node=ri.node; 03301 int c1,c2,c; 03302 Cube::EdgeCorners(ri.edgeIndex,c1,c2); 03303 while(node->parent){ 03304 c=int(node-node->parent->children); 03305 if(c!=c1 && c!=c2){return 0;} 03306 if(!MarchingCubes::HasEdgeRoots(node->parent->nodeData.mcIndex,ri.edgeIndex)){ 03307 if(c==c1){return GetRootIndex(&node->parent->children[c2],ri.edgeIndex,maxDepth,pair);} 03308 else{return GetRootIndex(&node->parent->children[c1],ri.edgeIndex,maxDepth,pair);} 03309 } 03310 node=node->parent; 03311 } 03312 return 0; 03313 03314 } 03315 template<int Degree> 03316 int Octree< Degree >::GetRootIndex( const RootInfo& ri , RootData& rootData , CoredPointIndex& index ) 03317 { 03318 long long key = ri.key; 03319 hash_map< long long , int >::iterator rootIter; 03320 rootIter = rootData.boundaryRoots.find( key ); 03321 if( rootIter!=rootData.boundaryRoots.end() ) 03322 { 03323 index.inCore = 1; 03324 index.index = rootIter->second; 03325 return 1; 03326 } 03327 else if( rootData.interiorRoots ) 03328 { 03329 int eIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ]; 03330 if( rootData.edgesSet[ eIndex ] ) 03331 { 03332 index.inCore = 0; 03333 index.index = rootData.interiorRoots[ eIndex ]; 03334 return 1; 03335 } 03336 } 03337 return 0; 03338 } 03339 template< int Degree > 03340 int Octree< Degree >::SetMCRootPositions( TreeOctNode* node , int sDepth , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , RootData& rootData , 03341 std::vector< Point3D< float > >* interiorPositions , CoredMeshData* mesh , const Real* metSolution , int nonLinearFit ) 03342 { 03343 Point3D< Real > position; 03344 int eIndex; 03345 RootInfo ri; 03346 int count=0; 03347 if( !MarchingCubes::HasRoots( node->nodeData.mcIndex ) ) return 0; 03348 for( int i=0 ; i<DIMENSION ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ ) 03349 { 03350 long long key; 03351 eIndex = Cube::EdgeIndex( i , j , k ); 03352 if( GetRootIndex( node , eIndex , fData.depth , ri ) ) 03353 { 03354 key = ri.key; 03355 if( !rootData.interiorRoots || IsBoundaryEdge( node , i , j , k , sDepth ) ) 03356 { 03357 poisson::hash_map< long long , int >::iterator iter , end; 03358 // Check if the root has already been set 03359 #pragma omp critical (boundary_roots_hash_access) 03360 { 03361 iter = rootData.boundaryRoots.find( key ); 03362 end = rootData.boundaryRoots.end(); 03363 } 03364 if( iter==end ) 03365 { 03366 // Get the root information 03367 GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit ); 03368 // Add the root if it hasn't been added already 03369 #pragma omp critical (boundary_roots_hash_access) 03370 { 03371 iter = rootData.boundaryRoots.find( key ); 03372 end = rootData.boundaryRoots.end(); 03373 if( iter==end ) 03374 { 03375 mesh->inCorePoints.push_back( position ); 03376 rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() ) - 1; 03377 } 03378 } 03379 if( iter==end ) count++; 03380 } 03381 } 03382 else 03383 { 03384 int nodeEdgeIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ]; 03385 if( !rootData.edgesSet[ nodeEdgeIndex ] ) 03386 { 03387 // Get the root information 03388 GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit ); 03389 // Add the root if it hasn't been added already 03390 #pragma omp critical (add_point_access) 03391 { 03392 if( !rootData.edgesSet[ nodeEdgeIndex ] ) 03393 { 03394 rootData.interiorRoots[ nodeEdgeIndex ] = mesh->addOutOfCorePoint( position ); 03395 interiorPositions->push_back( position ); 03396 rootData.edgesSet[ nodeEdgeIndex ] = 1; 03397 count++; 03398 } 03399 } 03400 } 03401 } 03402 } 03403 } 03404 return count; 03405 } 03406 template<int Degree> 03407 int Octree< Degree >::SetBoundaryMCRootPositions( int sDepth , Real isoValue , RootData& rootData , CoredMeshData* mesh , int nonLinearFit ) 03408 { 03409 Point3D< Real > position; 03410 int i,j,k,eIndex,hits=0; 03411 RootInfo ri; 03412 int count=0; 03413 TreeOctNode* node; 03414 03415 node = tree.nextLeaf(); 03416 while( node ) 03417 { 03418 if( MarchingCubes::HasRoots( node->nodeData.mcIndex ) ) 03419 { 03420 hits=0; 03421 for( i=0 ; i<DIMENSION ; i++ ) 03422 for( j=0 ; j<2 ; j++ ) 03423 for( k=0 ; k<2 ; k++ ) 03424 if( IsBoundaryEdge( node , i , j , k , sDepth ) ) 03425 { 03426 hits++; 03427 long long key; 03428 eIndex = Cube::EdgeIndex( i , j , k ); 03429 if( GetRootIndex( node , eIndex , fData.depth , ri ) ) 03430 { 03431 key = ri.key; 03432 if( rootData.boundaryRoots.find(key)==rootData.boundaryRoots.end() ) 03433 { 03434 GetRoot( ri , isoValue , position , rootData , sDepth , nonLinearFit ); 03435 mesh->inCorePoints.push_back( position ); 03436 rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() )-1; 03437 count++; 03438 } 03439 } 03440 } 03441 } 03442 if( hits ) node=tree.nextLeaf(node); 03443 else node=tree.nextBranch(node); 03444 } 03445 return count; 03446 } 03447 template<int Degree> 03448 void Octree< Degree >::GetMCIsoEdges( TreeOctNode* node , int sDepth , std::vector< std::pair< RootInfo , RootInfo > >& edges ) 03449 { 03450 TreeOctNode* temp; 03451 int count=0 , tris=0; 03452 int isoTri[ DIMENSION * MarchingCubes::MAX_TRIANGLES ]; 03453 FaceEdgesFunction fef; 03454 int ref , fIndex; 03455 hash_map< long long , std::pair< RootInfo , int > >::iterator iter; 03456 hash_map< long long , std::pair< RootInfo , int > > vertexCount; 03457 03458 fef.edges = &edges; 03459 fef.maxDepth = fData.depth; 03460 fef.vertexCount = &vertexCount; 03461 count = MarchingCubes::AddTriangleIndices( node->nodeData.mcIndex , isoTri ); 03462 for( fIndex=0 ; fIndex<Cube::NEIGHBORS ; fIndex++ ) 03463 { 03464 ref = Cube::FaceReflectFaceIndex( fIndex , fIndex ); 03465 fef.fIndex = ref; 03466 temp = node->faceNeighbor( fIndex ); 03467 // If the face neighbor exists and has higher resolution than the current node, 03468 // get the iso-curve from the neighbor 03469 if( temp && temp->children && !IsBoundaryFace( node , fIndex , sDepth ) ) temp->processNodeFaces( temp , &fef , ref ); 03470 // Otherwise, get it from the node 03471 else 03472 { 03473 RootInfo ri1 , ri2; 03474 for( int j=0 ; j<count ; j++ ) 03475 for( int k=0 ; k<3 ; k++ ) 03476 if( fIndex==Cube::FaceAdjacentToEdges( isoTri[j*3+k] , isoTri[j*3+((k+1)%3)] ) ) 03477 if( GetRootIndex( node , isoTri[j*3+k] , fData.depth , ri1 ) && GetRootIndex( node , isoTri[j*3+((k+1)%3)] , fData.depth , ri2 ) ) 03478 { 03479 long long key1 = ri1.key , key2 = ri2.key; 03480 edges.push_back( std::pair< RootInfo , RootInfo >( ri1 , ri2 ) ); 03481 iter=vertexCount.find( key1 ); 03482 if( iter==vertexCount.end() ) 03483 { 03484 vertexCount[key1].first = ri1; 03485 vertexCount[key1].second = 0; 03486 } 03487 iter=vertexCount.find( key2 ); 03488 if( iter==vertexCount.end() ) 03489 { 03490 vertexCount[key2].first = ri2; 03491 vertexCount[key2].second = 0; 03492 } 03493 vertexCount[key1].second++; 03494 vertexCount[key2].second--; 03495 } 03496 else 03497 { 03498 int r1 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+k] ); 03499 int r2 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+((k+1)%3)] ); 03500 fprintf( stderr , "Bad Edge 2: %d %d\t%d %d\n" , ri1.key , ri2.key , r1 , r2 ); 03501 } 03502 } 03503 } 03504 for( int i=0 ; i<int(edges.size()) ; i++ ) 03505 { 03506 iter = vertexCount.find( edges[i].first.key ); 03507 if( iter==vertexCount.end() ) printf( "Could not find vertex: %lld\n" , edges[i].first ); 03508 else if( vertexCount[ edges[i].first.key ].second ) 03509 { 03510 RootInfo ri; 03511 GetRootPair( vertexCount[edges[i].first.key].first , fData.depth , ri ); 03512 long long key = ri.key; 03513 iter = vertexCount.find( key ); 03514 if( iter==vertexCount.end() ) 03515 { 03516 int d , off[3]; 03517 node->depthAndOffset( d , off ); 03518 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] ); 03519 } 03520 else 03521 { 03522 edges.push_back( std::pair< RootInfo , RootInfo >( ri , edges[i].first ) ); 03523 vertexCount[ key ].second++; 03524 vertexCount[ edges[i].first.key ].second--; 03525 } 03526 } 03527 03528 iter = vertexCount.find( edges[i].second.key ); 03529 if( iter==vertexCount.end() ) printf( "Could not find vertex: %lld\n" , edges[i].second ); 03530 else if( vertexCount[edges[i].second.key].second ) 03531 { 03532 RootInfo ri; 03533 GetRootPair( vertexCount[edges[i].second.key].first , fData.depth , ri ); 03534 long long key = ri.key; 03535 iter=vertexCount.find( key ); 03536 if( iter==vertexCount.end() ) 03537 { 03538 int d , off[3]; 03539 node->depthAndOffset( d , off ); 03540 printf( "Vertex pair not in list 2\t[%d] (%d %d %d)\n" , d , off[0] , off[1] , off[2] ); 03541 } 03542 else 03543 { 03544 edges.push_back( std::pair< RootInfo , RootInfo >( edges[i].second , ri ) ); 03545 vertexCount[key].second--; 03546 vertexCount[ edges[i].second.key ].second++; 03547 } 03548 } 03549 } 03550 } 03551 template<int Degree> 03552 #if MISHA_DEBUG 03553 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 ) 03554 #else // !MISHA_DEBUG 03555 int Octree< Degree >::GetMCIsoTriangles( TreeOctNode* node , CoredMeshData* mesh , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , int offSet , int sDepth , bool addBarycenter , bool polygonMesh ) 03556 #endif // MISHA_DEBUG 03557 { 03558 int tris=0; 03559 std::vector< std::pair< RootInfo , RootInfo > > edges; 03560 std::vector< std::vector< std::pair< RootInfo , RootInfo > > > edgeLoops; 03561 GetMCIsoEdges( node , sDepth , edges ); 03562 03563 GetEdgeLoops( edges , edgeLoops ); 03564 for( int i=0 ; i<int(edgeLoops.size()) ; i++ ) 03565 { 03566 CoredPointIndex p; 03567 std::vector<CoredPointIndex> edgeIndices; 03568 for( int j=0 ; j<int(edgeLoops[i].size()) ; j++ ) 03569 { 03570 if( !GetRootIndex( edgeLoops[i][j].first , rootData , p ) ) printf( "Bad Point Index\n" ); 03571 else edgeIndices.push_back( p ); 03572 } 03573 #if MISHA_DEBUG 03574 tris += AddTriangles( mesh , edgeIndices , interiorPositions , offSet , polygonMesh , barycenters ); 03575 #else // !MISHA_DEBUG 03576 tris += AddTriangles( mesh , edgeIndices , interiorPositions , offSet , addBarycenter , polygonMesh ); 03577 #endif // MISHA_DEBUG 03578 } 03579 return tris; 03580 } 03581 03582 template< int Degree > 03583 int Octree< Degree >::GetEdgeLoops( std::vector< std::pair< RootInfo , RootInfo > >& edges , std::vector< std::vector< std::pair< RootInfo , RootInfo > > >& loops ) 03584 { 03585 int loopSize=0; 03586 long long frontIdx , backIdx; 03587 std::pair< RootInfo , RootInfo > e , temp; 03588 loops.clear(); 03589 03590 while( edges.size() ) 03591 { 03592 std::vector< std::pair< RootInfo , RootInfo > > front , back; 03593 e = edges[0]; 03594 loops.resize( loopSize+1 ); 03595 edges[0] = edges.back(); 03596 edges.pop_back(); 03597 frontIdx = e.second.key; 03598 backIdx = e.first.key; 03599 for( int j=int(edges.size())-1 ; j>=0 ; j-- ) 03600 { 03601 if( edges[j].first.key==frontIdx || edges[j].second.key==frontIdx ) 03602 { 03603 if( edges[j].first.key==frontIdx ) temp = edges[j]; 03604 else temp.first = edges[j].second , temp.second = edges[j].first; 03605 frontIdx = temp.second.key; 03606 front.push_back(temp); 03607 edges[j] = edges.back(); 03608 edges.pop_back(); 03609 j = int(edges.size()); 03610 } 03611 else if( edges[j].first.key==backIdx || edges[j].second.key==backIdx ) 03612 { 03613 if( edges[j].second.key==backIdx ) temp = edges[j]; 03614 else temp.first = edges[j].second , temp.second = edges[j].first; 03615 backIdx = temp.first.key; 03616 back.push_back(temp); 03617 edges[j] = edges.back(); 03618 edges.pop_back(); 03619 j = int(edges.size()); 03620 } 03621 } 03622 for( int j=int(back.size())-1 ; j>=0 ; j-- ) loops[loopSize].push_back( back[j] ); 03623 loops[loopSize].push_back(e); 03624 for( int j=0 ; j<int(front.size()) ; j++ ) loops[loopSize].push_back( front[j] ); 03625 loopSize++; 03626 } 03627 return int(loops.size()); 03628 } 03629 template<int Degree> 03630 #if MISHA_DEBUG 03631 int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool polygonMesh , std::vector< Point3D< float > >* barycenters ) 03632 #else // !MISHA_DEBUG 03633 int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool addBarycenter , bool polygonMesh ) 03634 #endif // MISHA_DEBUG 03635 { 03636 MinimalAreaTriangulation< float > MAT; 03637 std::vector< Point3D< float > > vertices; 03638 std::vector< TriangleIndex > triangles; 03639 if( polygonMesh ) 03640 { 03641 std::vector< CoredVertexIndex > vertices( edges.size() ); 03642 for( int i=0 ; i<int(edges.size()) ; i++ ) 03643 { 03644 vertices[i].idx = edges[i].index; 03645 vertices[i].inCore = (edges[i].inCore!=0); 03646 } 03647 #pragma omp critical (add_polygon_access) 03648 { 03649 mesh->addPolygon( vertices ); 03650 } 03651 return 1; 03652 } 03653 if( edges.size()>3 ) 03654 { 03655 bool isCoplanar = false; 03656 03657 #if MISHA_DEBUG 03658 if( barycenters ) 03659 #else // !MISHA_DEBUG 03660 if( addBarycenter ) 03661 #endif // MISHA_DEBUG 03662 for( int i=0 ; i<int(edges.size()) ; i++ ) 03663 for( int j=0 ; j<i ; j++ ) 03664 if( (i+1)%edges.size()!=j && (j+1)%edges.size()!=i ) 03665 { 03666 Point3D< Real > v1 , v2; 03667 if( edges[i].inCore ) v1 = mesh->inCorePoints[ edges[i].index ]; 03668 else v1 = (*interiorPositions)[ edges[i].index-offSet ]; 03669 if( edges[j].inCore ) v2 = mesh->inCorePoints[ edges[j].index ]; 03670 else v2 = (*interiorPositions)[ edges[j].index-offSet ]; 03671 for( int k=0 ; k<3 ; k++ ) if( v1[k]==v2[k] ) isCoplanar = true; 03672 } 03673 if( isCoplanar ) 03674 { 03675 Point3D< Real > c; 03676 c[0] = c[1] = c[2] = 0; 03677 for( int i=0 ; i<int(edges.size()) ; i++ ) 03678 { 03679 Point3D<Real> p; 03680 if(edges[i].inCore) p = mesh->inCorePoints[edges[i].index ]; 03681 else p = (*interiorPositions)[edges[i].index-offSet]; 03682 c += p; 03683 } 03684 c /= Real( edges.size() ); 03685 int cIdx; 03686 #pragma omp critical (add_point_access) 03687 { 03688 cIdx = mesh->addOutOfCorePoint( c ); 03689 #if MISHA_DEBUG 03690 barycenters->push_back( c ); 03691 #else // !MISHA_DEBUG 03692 interiorPositions->push_back( c ); 03693 #endif // MISHA_DEBUG 03694 } 03695 for( int i=0 ; i<int(edges.size()) ; i++ ) 03696 { 03697 std::vector< CoredVertexIndex > vertices( 3 ); 03698 vertices[0].idx = edges[i ].index; 03699 vertices[1].idx = edges[(i+1)%edges.size()].index; 03700 vertices[2].idx = cIdx; 03701 vertices[0].inCore = (edges[i ].inCore!=0); 03702 vertices[1].inCore = (edges[(i+1)%edges.size()].inCore!=0); 03703 vertices[2].inCore = 0; 03704 #pragma omp critical (add_polygon_access) 03705 { 03706 mesh->addPolygon( vertices ); 03707 } 03708 } 03709 return int( edges.size() ); 03710 } 03711 else 03712 { 03713 vertices.resize( edges.size() ); 03714 // Add the points 03715 for( int i=0 ; i<int(edges.size()) ; i++ ) 03716 { 03717 Point3D< Real > p; 03718 if( edges[i].inCore ) p = mesh->inCorePoints[edges[i].index ]; 03719 else p = (*interiorPositions)[edges[i].index-offSet]; 03720 vertices[i] = p; 03721 } 03722 MAT.GetTriangulation( vertices , triangles ); 03723 for( int i=0 ; i<int(triangles.size()) ; i++ ) 03724 { 03725 std::vector< CoredVertexIndex > _vertices( 3 ); 03726 for( int j=0 ; j<3 ; j++ ) 03727 { 03728 _vertices[j].idx = edges[ triangles[i].idx[j] ].index; 03729 _vertices[j].inCore = (edges[ triangles[i].idx[j] ].inCore!=0); 03730 } 03731 #pragma omp critical (add_polygon_access) 03732 { 03733 mesh->addPolygon( _vertices ); 03734 } 03735 } 03736 } 03737 } 03738 else if( edges.size()==3 ) 03739 { 03740 std::vector< CoredVertexIndex > vertices( 3 ); 03741 for( int i=0 ; i<3 ; i++ ) 03742 { 03743 vertices[i].idx = edges[i].index; 03744 vertices[i].inCore = (edges[i].inCore!=0); 03745 } 03746 #pragma omp critical (add_polygon_access) 03747 mesh->addPolygon( vertices ); 03748 } 03749 return int(edges.size())-2; 03750 } 03751 template< int Degree > 03752 Real* Octree< Degree >::GetSolutionGrid( int& res , float isoValue , int depth ) 03753 { 03754 if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth(); 03755 BSplineData< Degree , Real > fData; 03756 fData.set( depth ); 03757 fData.setValueTables( fData.VALUE_FLAG ); 03758 res = 1<<depth; 03759 Real* values = new float[ res * res * res ]; 03760 memset( values , 0 , sizeof( float ) * res * res * res ); 03761 03762 for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode( n ) ) 03763 { 03764 if( n->d>depth ) continue; 03765 if( n->d<_minDepth ) continue; 03766 int d , idx[3] , start[3] , end[3]; 03767 n->depthAndOffset( d , idx ); 03768 for( int i=0 ; i<3 ; i++ ) 03769 { 03770 // Get the index of the functions 03771 idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] ); 03772 // Figure out which samples fall into the range 03773 fData.setSampleSpan( idx[i] , start[i] , end[i] ); 03774 // We only care about the odd indices 03775 if( !(start[i]&1) ) start[i]++; 03776 if( !( end[i]&1) ) end[i]--; 03777 } 03778 Real coefficient = n->nodeData.solution; 03779 for( int x=start[0] ; x<=end[0] ; x+=2 ) 03780 for( int y=start[1] ; y<=end[1] ; y+=2 ) 03781 for( int z=start[2] ; z<=end[2] ; z+=2 ) 03782 { 03783 int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1; 03784 values[ zz*res*res + yy*res + xx ] += 03785 coefficient * 03786 fData.valueTables[ idx[0]+x*fData.functionCount ] * 03787 fData.valueTables[ idx[1]+y*fData.functionCount ] * 03788 fData.valueTables[ idx[2]+z*fData.functionCount ]; 03789 } 03790 } 03791 for( int i=0 ; i<res*res*res ; i++ ) values[i] -= isoValue; 03792 03793 return values; 03794 } 03795 template< int Degree > 03796 Real* Octree< Degree >::GetWeightGrid( int& res , int depth ) 03797 { 03798 if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth(); 03799 res = 1<<tree.maxDepth(); 03800 Real* values = new float[ res * res * res ]; 03801 memset( values , 0 , sizeof( float ) * res * res * res ); 03802 03803 for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode( n ) ) 03804 { 03805 if( n->d>depth ) continue; 03806 int d , idx[3] , start[3] , end[3]; 03807 n->depthAndOffset( d , idx ); 03808 for( int i=0 ; i<3 ; i++ ) 03809 { 03810 // Get the index of the functions 03811 idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] ); 03812 // Figure out which samples fall into the range 03813 fData.setSampleSpan( idx[i] , start[i] , end[i] ); 03814 // We only care about the odd indices 03815 if( !(start[i]&1) ) start[i]++; 03816 if( !( end[i]&1) ) end[i]--; 03817 } 03818 for( int x=start[0] ; x<=end[0] ; x+=2 ) 03819 for( int y=start[1] ; y<=end[1] ; y+=2 ) 03820 for( int z=start[2] ; z<=end[2] ; z+=2 ) 03821 { 03822 int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1; 03823 values[ zz*res*res + yy*res + xx ] += 03824 n->nodeData.centerWeightContribution * 03825 fData.valueTables[ idx[0]+x*fData.functionCount ] * 03826 fData.valueTables[ idx[1]+y*fData.functionCount ] * 03827 fData.valueTables[ idx[2]+z*fData.functionCount ]; 03828 } 03829 } 03830 return values; 03831 } 03832 03833 //////////////// 03834 // VertexData // 03835 //////////////// 03836 long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth){ 03837 int idx[DIMENSION]; 03838 return CenterIndex(node,maxDepth,idx); 03839 } 03840 long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth,int idx[DIMENSION]){ 03841 int d,o[3]; 03842 node->depthAndOffset(d,o); 03843 for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);} 03844 return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; 03845 } 03846 long long VertexData::CenterIndex(int depth,const int offSet[DIMENSION],int maxDepth,int idx[DIMENSION]){ 03847 for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,depth+1,offSet[i]<<1,1);} 03848 return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; 03849 } 03850 long long VertexData::CornerIndex(const TreeOctNode* node,int cIndex,int maxDepth){ 03851 int idx[DIMENSION]; 03852 return CornerIndex(node,cIndex,maxDepth,idx); 03853 } 03854 long long VertexData::CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int idx[DIMENSION] ) 03855 { 03856 int x[DIMENSION]; 03857 Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] ); 03858 int d , o[3]; 03859 node->depthAndOffset( d , o ); 03860 for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , d , o[i] , x[i] ); 03861 return CornerIndexKey( idx ); 03862 } 03863 long long VertexData::CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int idx[DIMENSION] ) 03864 { 03865 int x[DIMENSION]; 03866 Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] ); 03867 for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , depth , offSet[i] , x[i] ); 03868 return CornerIndexKey( idx ); 03869 } 03870 long long VertexData::CornerIndexKey( const int idx[DIMENSION] ) 03871 { 03872 return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; 03873 } 03874 long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth){ 03875 int idx[DIMENSION]; 03876 return FaceIndex(node,fIndex,maxDepth,idx); 03877 } 03878 long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth,int idx[DIMENSION]){ 03879 int dir,offset; 03880 Cube::FactorFaceIndex(fIndex,dir,offset); 03881 int d,o[3]; 03882 node->depthAndOffset(d,o); 03883 for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);} 03884 idx[dir]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,o[dir],offset); 03885 return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; 03886 } 03887 long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth){ 03888 int idx[DIMENSION]; 03889 return EdgeIndex(node,eIndex,maxDepth,idx); 03890 } 03891 long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth,int idx[DIMENSION]){ 03892 int o,i1,i2; 03893 int d,off[3]; 03894 node->depthAndOffset(d,off); 03895 for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,off[i]<<1,1);} 03896 Cube::FactorEdgeIndex(eIndex,o,i1,i2); 03897 switch(o){ 03898 case 0: 03899 idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1); 03900 idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2); 03901 break; 03902 case 1: 03903 idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1); 03904 idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2); 03905 break; 03906 case 2: 03907 idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1); 03908 idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2); 03909 break; 03910 }; 03911 return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30; 03912 } 03913 03914 03915 } 03916 } 03917 03918