Loading...
Searching...
No Matches
multi_grid_octree_data.hpp
/*
Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer. Redistributions in binary form must reproduce
the above copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution.
Neither the name of the Johns Hopkins University nor the names of its contributors
may be used to endorse or promote products derived from this software without specific
prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
DAMAGE.
*/
#include <unordered_map>
#include "poisson_exceptions.h"
#include "octree_poisson.h"
#include "mat.h"
#if defined WIN32 || defined _WIN32
#if !defined __MINGW32__
#include <intrin.h>
#endif
#endif
#define ITERATION_POWER 1.0/3
#define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12
#define SPLAT_ORDER 2
namespace pcl
{
namespace poisson
{
constexpr Real MATRIX_ENTRY_EPSILON = Real(0);
constexpr Real EPSILON=Real(1e-6);
constexpr Real ROUND_EPS = Real(1e-5);
void atomicOr(volatile int& dest, int value)
{
#if defined _WIN32 && !defined __MINGW32__
#if defined (_M_IX86)
_InterlockedOr( (long volatile*)&dest, value );
#else
InterlockedOr( (long volatile*)&dest , value );
#endif
#else // !_WIN32 || __MINGW32__
#pragma omp atomic
dest |= value;
#endif // _WIN32 && !__MINGW32__
}
/////////////////////
// SortedTreeNodes //
/////////////////////
SortedTreeNodes::SortedTreeNodes(void)
{
nodeCount=NULL;
treeNodes=NULL;
maxDepth=0;
}
SortedTreeNodes::~SortedTreeNodes(void){
delete[] nodeCount;
delete[] treeNodes;
nodeCount = NULL;
treeNodes = NULL;
}
void SortedTreeNodes::set( TreeOctNode& root )
{
delete[] nodeCount;
delete[] treeNodes;
maxDepth = root.maxDepth()+1;
nodeCount = new int[ maxDepth+1 ];
treeNodes = new TreeOctNode*[ root.nodes() ];
nodeCount[0] = 0 , nodeCount[1] = 1;
treeNodes[0] = &root;
for( int d=1 ; d<maxDepth ; d++ )
{
nodeCount[d+1] = nodeCount[d];
for( int i=nodeCount[d-1] ; i<nodeCount[d] ; i++ )
{
TreeOctNode* temp = treeNodes[i];
if( temp->children ) for( int c=0 ; c<8 ; c++ ) treeNodes[ nodeCount[d+1]++ ] = temp->children + c;
}
}
for( int i=0 ; i<nodeCount[maxDepth] ; i++ ) treeNodes[i]->nodeData.nodeIndex = i;
}
SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::operator[] ( const TreeOctNode* node ) { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
const SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::operator[] ( const TreeOctNode* node ) const { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::cornerIndices( const TreeOctNode* node ) { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
const SortedTreeNodes::CornerIndices& SortedTreeNodes::CornerTableData::cornerIndices( const TreeOctNode* node ) const { return cTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
void SortedTreeNodes::setCornerTable( CornerTableData& cData , const TreeOctNode* rootNode , int maxDepth , int threads ) const
{
if( threads<=0 ) threads = 1;
// The vector of per-depth node spans
std::vector< std::pair< int , int > > spans( this->maxDepth , std::pair< int , int >( -1 , -1 ) );
int minDepth , off[3];
rootNode->depthAndOffset( minDepth , off );
cData.offsets.resize( this->maxDepth , -1 );
int nodeCount = 0;
{
int start=rootNode->nodeData.nodeIndex , end=start;
for( int d=minDepth ; d<=maxDepth ; d++ )
{
spans[d] = std::pair< int , int >( start , end+1 );
cData.offsets[d] = nodeCount - spans[d].first;
nodeCount += spans[d].second - spans[d].first;
if( d<maxDepth )
{
while( start< end && !treeNodes[start]->children ) start++;
while( end> start && !treeNodes[end ]->children ) end--;
if( start==end && !treeNodes[start]->children ) break;
start = treeNodes[start]->children[0].nodeData.nodeIndex;
end = treeNodes[end ]->children[7].nodeData.nodeIndex;
}
}
}
cData.cTable.resize( nodeCount );
std::vector< int > count( threads );
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::ConstNeighborKey3 neighborKey;
neighborKey.set( maxDepth );
int offset = nodeCount * t * Cube::CORNERS;
count[t] = 0;
for( int d=minDepth ; d<=maxDepth ; d++ )
{
int start = spans[d].first , end = spans[d].second , width = end-start;
for( int i=start + (width*t)/threads ; i<start + (width*(t+1))/threads ; i++ )
{
TreeOctNode* node = treeNodes[i];
if( d<maxDepth && node->children ) continue;
const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , minDepth );
for( int c=0 ; c<Cube::CORNERS ; c++ ) // Iterate over the cell's corners
{
bool cornerOwner = true;
int x , y , z;
int ac = Cube::AntipodalCornerIndex( c ); // The index of the node relative to the corner
Cube::FactorCornerIndex( c , x , y , z );
for( int cc=0 ; cc<Cube::CORNERS ; cc++ ) // Iterate over the corner's cells
{
int xx , yy , zz;
Cube::FactorCornerIndex( cc , xx , yy , zz );
xx += x , yy += y , zz += z;
if( neighbors.neighbors[xx][yy][zz] )
if( cc<ac || ( d<maxDepth && neighbors.neighbors[xx][yy][zz]->children ) )
{
int _d , _off[3];
neighbors.neighbors[xx][yy][zz]->depthAndOffset( _d , _off );
_off[0] >>= (d-minDepth) , _off[1] >>= (d-minDepth) , _off[2] >>= (d-minDepth);
if( _off[0]==off[0] && _off[1]==off[1] && _off[2]==off[2] )
{
cornerOwner = false;
break;
}
else fprintf( stderr , "[WARNING] How did we leave the subtree?\n" );
}
}
if( cornerOwner )
{
const TreeOctNode* n = node;
int d = n->depth();
do
{
const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.neighbors[d];
// Set all the corner indices at the current depth
for( int cc=0 ; cc<Cube::CORNERS ; cc++ )
{
int xx , yy , zz;
Cube::FactorCornerIndex( cc , xx , yy , zz );
xx += x , yy += y , zz += z;
if( neighborKey.neighbors[d].neighbors[xx][yy][zz] )
cData[ neighbors.neighbors[xx][yy][zz] ][ Cube::AntipodalCornerIndex(cc) ] = count[t] + offset;
}
// If we are not at the root and the parent also has the corner
if( d==minDepth || n!=(n->parent->children+c) ) break;
n = n->parent;
d--;
}
while( 1 );
count[t]++;
}
}
}
}
}
cData.cCount = 0;
std::vector< int > offsets( threads+1 );
offsets[0] = 0;
for (int t = 0; t < threads; t++)
{
cData.cCount += count[t];
offsets[t + 1] = offsets[t] + count[t];
}
unsigned int paralellExceptionCount = 0;
#pragma omp parallel for num_threads( threads )
for (int t = 0; t < threads; t++)
{
for (int d = minDepth; d <= maxDepth; d++)
{
int start = spans[d].first, end = spans[d].second, width = end - start;
for (int i = start + (width*t) / threads; i < start + (width*(t + 1)) / threads; i++)
{
for (int c = 0; c < Cube::CORNERS; c++)
{
int& idx = cData[ treeNodes[i] ][c];
if ( idx<0 )
{
#pragma omp critical
{
// found unindexed corner
++paralellExceptionCount;
}
}
else
{
int div = idx / ( nodeCount*Cube::CORNERS );
int rem = idx % ( nodeCount*Cube::CORNERS );
idx = rem + offsets[div];
}
}
}
}
}
if (paralellExceptionCount > 0)
POISSON_THROW_EXCEPTION (pcl::poisson::PoissonOpenMPException, "Found " << paralellExceptionCount << " unindexed corner nodes during openMP loop execution.");
}
int SortedTreeNodes::getMaxCornerCount( const TreeOctNode* rootNode , int depth , int maxDepth , int threads ) const
{
if( threads<=0 ) threads = 1;
int res = 1<<depth;
std::vector< std::vector< int > > cornerCount( threads );
for( int t=0 ; t<threads ; t++ ) cornerCount[t].resize( res*res*res , 0 );
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
std::vector< int >& _cornerCount = cornerCount[t];
TreeOctNode::ConstNeighborKey3 neighborKey;
neighborKey.set( maxDepth );
int start = nodeCount[depth] , end = nodeCount[maxDepth+1] , range = end-start;
for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ )
{
TreeOctNode* node = treeNodes[start+i];
int d , off[3];
node->depthAndOffset( d , off );
if( d<maxDepth && node->children ) continue;
const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , depth );
for( int c=0 ; c<Cube::CORNERS ; c++ ) // Iterate over the cell's corners
{
bool cornerOwner = true;
int x , y , z;
int ac = Cube::AntipodalCornerIndex( c ); // The index of the node relative to the corner
Cube::FactorCornerIndex( c , x , y , z );
for( int cc=0 ; cc<Cube::CORNERS ; cc++ ) // Iterate over the corner's cells
{
int xx , yy , zz;
Cube::FactorCornerIndex( cc , xx , yy , zz );
xx += x , yy += y , zz += z;
if( neighbors.neighbors[xx][yy][zz] )
if( cc<ac || ( d<maxDepth && neighbors.neighbors[xx][yy][zz]->children ) )
{
cornerOwner = false;
break;
}
}
if( cornerOwner ) _cornerCount[ ( ( off[0]>>(d-depth) ) * res * res) + ( ( off[1]>>(d-depth) ) * res) + ( off[2]>>(d-depth) ) ]++;
}
}
}
int maxCount = 0;
for( int i=0 ; i<res*res*res ; i++ )
{
int c = 0;
for( int t=0 ; t<threads ; t++ ) c += cornerCount[t][i];
maxCount = std::max< int >( maxCount , c );
}
return maxCount;
}
SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::operator[] ( const TreeOctNode* node ) { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
const SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::operator[] ( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::edgeIndices( const TreeOctNode* node ) { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
const SortedTreeNodes::EdgeIndices& SortedTreeNodes::EdgeTableData::edgeIndices( const TreeOctNode* node ) const { return eTable[ node->nodeData.nodeIndex + offsets[node->d] ]; }
void SortedTreeNodes::setEdgeTable( EdgeTableData& eData , const TreeOctNode* rootNode , int maxDepth , int threads )
{
if( threads<=0 ) threads = 1;
std::vector< std::pair< int , int > > spans( this->maxDepth , std::pair< int , int >( -1 , -1 ) );
int minDepth , off[3];
rootNode->depthAndOffset( minDepth , off );
eData.offsets.resize( this->maxDepth , -1 );
int nodeCount = 0;
{
int start=rootNode->nodeData.nodeIndex , end=start;
for( int d=minDepth ; d<=maxDepth ; d++ )
{
spans[d] = std::pair< int , int >( start , end+1 );
eData.offsets[d] = nodeCount - spans[d].first;
nodeCount += spans[d].second - spans[d].first;
if( d<maxDepth )
{
while( start< end && !treeNodes[start]->children ) start++;
while( end> start && !treeNodes[end ]->children ) end--;
if( start==end && !treeNodes[start]->children ) break;
start = treeNodes[start]->children[0].nodeData.nodeIndex;
end = treeNodes[end ]->children[7].nodeData.nodeIndex;
}
}
}
eData.eTable.resize( nodeCount );
std::vector< int > count( threads );
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::ConstNeighborKey3 neighborKey;
neighborKey.set( maxDepth );
int offset = nodeCount * t * Cube::EDGES;
count[t] = 0;
for( int d=minDepth ; d<=maxDepth ; d++ )
{
int start = spans[d].first , end = spans[d].second , width = end-start;
for( int i=start + (width*t)/threads ; i<start + (width*(t+1))/threads ; i++ )
{
TreeOctNode* node = treeNodes[i];
const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , minDepth );
for( int e=0 ; e<Cube::EDGES ; e++ )
{
bool edgeOwner = true;
int o , i , j;
Cube::FactorEdgeIndex( e , o , i , j );
int ac = Square::AntipodalCornerIndex( Square::CornerIndex( i , j ) );
for( int cc=0 ; cc<Square::CORNERS ; cc++ )
{
int ii , jj , x , y , z;
Square::FactorCornerIndex( cc , ii , jj );
ii += i , jj += j;
switch( o )
{
case 0: y = ii , z = jj , x = 1 ; break;
case 1: x = ii , z = jj , y = 1 ; break;
case 2: x = ii , y = jj , z = 1 ; break;
}
if( neighbors.neighbors[x][y][z] && cc<ac ) { edgeOwner = false ; break; }
}
if( edgeOwner )
{
// Set all edge indices
for( int cc=0 ; cc<Square::CORNERS ; cc++ )
{
int ii , jj , aii , ajj , x , y , z;
Square::FactorCornerIndex( cc , ii , jj );
Square::FactorCornerIndex( Square::AntipodalCornerIndex( cc ) , aii , ajj );
ii += i , jj += j;
switch( o )
{
case 0: y = ii , z = jj , x = 1 ; break;
case 1: x = ii , z = jj , y = 1 ; break;
case 2: x = ii , y = jj , z = 1 ; break;
}
if( neighbors.neighbors[x][y][z] )
eData[ neighbors.neighbors[x][y][z] ][ Cube::EdgeIndex( o , aii , ajj ) ] = count[t]+offset;
}
count[t]++;
}
}
}
}
}
eData.eCount = 0;
std::vector< int > offsets( threads+1 );
offsets[0] = 0;
for (int t = 0; t < threads; t++)
{
eData.eCount += count[t];
offsets[t + 1] = offsets[t] + count[t];
}
unsigned int paralellExceptionCount = 0;
#pragma omp parallel for num_threads( threads )
for (int t = 0; t < threads; t++)
{
for (int d = minDepth; d <= maxDepth; d++)
{
int start = spans[d].first, end = spans[d].second, width = end - start;
for (int i = start + (width*t) / threads; i < start + (width*(t + 1)) / threads; i++)
{
for (int e = 0; e < Cube::EDGES; e++)
{
int& idx = eData[treeNodes[i]][e];
if (idx < 0)
{
#pragma omp critical
{
// found unindexed edge
++paralellExceptionCount;
}
}
else
{
int div = idx / ( nodeCount*Cube::EDGES );
int rem = idx % ( nodeCount*Cube::EDGES );
idx = rem + offsets[div];
}
}
}
}
}
if(paralellExceptionCount > 0)
POISSON_THROW_EXCEPTION (pcl::poisson::PoissonOpenMPException, "Found " << paralellExceptionCount << " unindexed edges during openMP loop execution.");
}
int SortedTreeNodes::getMaxEdgeCount( const TreeOctNode* rootNode , int depth , int threads ) const
{
if( threads<=0 ) threads = 1;
int res = 1<<depth;
std::vector< std::vector< int > > edgeCount( threads );
for( int t=0 ; t<threads ; t++ ) edgeCount[t].resize( res*res*res , 0 );
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
std::vector< int >& _edgeCount = edgeCount[t];
TreeOctNode::ConstNeighborKey3 neighborKey;
neighborKey.set( maxDepth-1 );
int start = nodeCount[depth] , end = nodeCount[maxDepth] , range = end-start;
for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ )
{
TreeOctNode* node = treeNodes[start+i];
const TreeOctNode::ConstNeighbors3& neighbors = neighborKey.getNeighbors( node , depth );
int d , off[3];
node->depthAndOffset( d , off );
for( int e=0 ; e<Cube::EDGES ; e++ )
{
bool edgeOwner = true;
int o , i , j;
Cube::FactorEdgeIndex( e , o , i , j );
int ac = Square::AntipodalCornerIndex( Square::CornerIndex( i , j ) );
for( int cc=0 ; cc<Square::CORNERS ; cc++ )
{
int ii , jj , x , y , z;
Square::FactorCornerIndex( cc , ii , jj );
ii += i , jj += j;
switch( o )
{
case 0: y = ii , z = jj , x = 1 ; break;
case 1: x = ii , z = jj , y = 1 ; break;
case 2: x = ii , y = jj , z = 1 ; break;
}
if( neighbors.neighbors[x][y][z] && cc<ac ) { edgeOwner = false ; break; }
}
if( edgeOwner ) _edgeCount[ ( ( off[0]>>(d-depth) ) * res * res) + ( ( off[1]>>(d-depth) ) * res) + ( off[2]>>(d-depth) ) ]++;
}
}
}
int maxCount = 0;
for( int i=0 ; i<res*res*res ; i++ )
{
int c = 0;
for( int t=0 ; t<threads ; t++ ) c += edgeCount[t][i];
maxCount = std::max< int >( maxCount , c );
}
return maxCount;
}
//////////////////
// TreeNodeData //
//////////////////
int TreeNodeData::UseIndex=1;
TreeNodeData::TreeNodeData( void )
{
if( UseIndex )
{
nodeIndex = -1;
centerWeightContribution=0;
}
else mcIndex=0;
normalIndex = -1;
constraint = solution = 0;
pointIndex = -1;
}
TreeNodeData::~TreeNodeData( void ) { }
////////////
// Octree //
////////////
template<int Degree>
double Octree<Degree>::maxMemoryUsage=0;
template<int Degree>
double Octree<Degree>::MemoryUsage(void){
double mem = 0;//double( MemoryInfo::Usage() ) / (1<<20);
if(mem>maxMemoryUsage){maxMemoryUsage=mem;}
return mem;
}
template<int Degree>
Octree<Degree>::Octree(void)
{
threads = 1;
radius = 0;
width = 0;
postNormalSmooth = 0;
_constrainValues = false;
}
template<int Degree>
int Octree<Degree>::NonLinearSplatOrientedPoint( TreeOctNode* node , const Point3D<Real>& position , const Point3D<Real>& normal )
{
double x , dxdy , dxdydz , dx[DIMENSION][SPLAT_ORDER+1];
int off[3];
TreeOctNode::Neighbors3& neighbors = neighborKey.setNeighbors( node );
double width;
Point3D<Real> center;
Real w;
node->centerAndWidth( center , w );
width=w;
for( int i=0 ; i<3 ; i++ )
{
#if SPLAT_ORDER==2
off[i] = 0;
x = ( center[i] - position[i] - width ) / width;
dx[i][0] = 1.125+1.500*x+0.500*x*x;
x = ( center[i] - position[i] ) / width;
dx[i][1] = 0.750 - x*x;
dx[i][2] = 1. - dx[i][1] - dx[i][0];
#elif SPLAT_ORDER==1
x = ( position[i] - center[i] ) / width;
if( x<0 )
{
off[i] = 0;
dx[i][0] = -x;
}
else
{
off[i] = 1;
dx[i][0] = 1. - x;
}
dx[i][1] = 1. - dx[i][0];
#elif SPLAT_ORDER==0
off[i] = 1;
dx[i][0] = 1.;
#else
# error Splat order not supported
#endif // SPLAT_ORDER
}
for( int i=off[0] ; i<=off[0]+SPLAT_ORDER ; i++ ) for( int j=off[1] ; j<=off[1]+SPLAT_ORDER ; j++ )
{
dxdy = dx[0][i] * dx[1][j];
for( int k=off[2] ; k<=off[2]+SPLAT_ORDER ; k++ )
if( neighbors.neighbors[i][j][k] )
{
dxdydz = dxdy * dx[2][k];
TreeOctNode* _node = neighbors.neighbors[i][j][k];
int idx =_node->nodeData.normalIndex;
if( idx<0 )
{
Point3D<Real> n;
n[0] = n[1] = n[2] = 0;
_node->nodeData.nodeIndex = 0;
idx = _node->nodeData.normalIndex = int(normals->size());
normals->push_back(n);
}
(*normals)[idx] += normal * Real( dxdydz );
}
}
return 0;
}
template<int Degree>
Real Octree<Degree>::NonLinearSplatOrientedPoint( const Point3D<Real>& position , const Point3D<Real>& normal , int splatDepth , Real samplesPerNode ,
int minDepth , int maxDepth )
{
double dx;
Point3D<Real> n;
TreeOctNode* temp;
int cnt=0;
double width;
Point3D< Real > myCenter;
Real myWidth;
myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5);
myWidth = Real(1.0);
temp = &tree;
while( temp->depth()<splatDepth )
{
if( !temp->children )
{
fprintf( stderr , "Octree<Degree>::NonLinearSplatOrientedPoint error\n" );
return -1;
}
int cIndex=TreeOctNode::CornerIndex(myCenter,position);
temp=&temp->children[cIndex];
myWidth/=2;
if(cIndex&1) myCenter[0] += myWidth/2;
else myCenter[0] -= myWidth/2;
if(cIndex&2) myCenter[1] += myWidth/2;
else myCenter[1] -= myWidth/2;
if(cIndex&4) myCenter[2] += myWidth/2;
else myCenter[2] -= myWidth/2;
}
Real alpha,newDepth;
NonLinearGetSampleDepthAndWeight( temp , position , samplesPerNode , newDepth , alpha );
if( newDepth<minDepth ) newDepth=Real(minDepth);
if( newDepth>maxDepth ) newDepth=Real(maxDepth);
int topDepth=int(ceil(newDepth));
dx = 1.0-(topDepth-newDepth);
if( topDepth<=minDepth )
{
topDepth=minDepth;
dx=1;
}
else if( topDepth>maxDepth )
{
topDepth=maxDepth;
dx=1;
}
while( temp->depth()>topDepth ) temp=temp->parent;
while( temp->depth()<topDepth )
{
if(!temp->children) temp->initChildren();
int cIndex=TreeOctNode::CornerIndex(myCenter,position);
temp=&temp->children[cIndex];
myWidth/=2;
if(cIndex&1) myCenter[0] += myWidth/2;
else myCenter[0] -= myWidth/2;
if(cIndex&2) myCenter[1] += myWidth/2;
else myCenter[1] -= myWidth/2;
if(cIndex&4) myCenter[2] += myWidth/2;
else myCenter[2] -= myWidth/2;
}
width = 1.0 / ( 1<<temp->depth() );
n = normal * alpha / Real( pow( width , 3 ) ) * Real( dx );
NonLinearSplatOrientedPoint( temp , position , n );
if( fabs(1.0-dx) > EPSILON )
{
dx = Real(1.0-dx);
temp = temp->parent;
width = 1.0 / ( 1<<temp->depth() );
n = normal * alpha / Real( pow( width , 3 ) ) * Real( dx );
NonLinearSplatOrientedPoint( temp , position , n );
}
return alpha;
}
template<int Degree>
void Octree<Degree>::NonLinearGetSampleDepthAndWeight(TreeOctNode* node,const Point3D<Real>& position,Real samplesPerNode,Real& depth,Real& weight){
TreeOctNode* temp=node;
weight = Real(1.0)/NonLinearGetSampleWeight(temp,position);
if( weight>=samplesPerNode ) depth=Real( temp->depth() + log( weight / samplesPerNode ) / log(double(1<<(DIMENSION-1))) );
else
{
Real oldAlpha,newAlpha;
oldAlpha=newAlpha=weight;
while( newAlpha<samplesPerNode && temp->parent )
{
temp=temp->parent;
oldAlpha=newAlpha;
newAlpha=Real(1.0)/NonLinearGetSampleWeight(temp,position);
}
depth = Real( temp->depth() + log( newAlpha / samplesPerNode ) / log( newAlpha / oldAlpha ) );
}
weight=Real(pow(double(1<<(DIMENSION-1)),-double(depth)));
}
template<int Degree>
Real Octree<Degree>::NonLinearGetSampleWeight( TreeOctNode* node , const Point3D<Real>& position )
{
Real weight=0;
double x,dxdy,dx[DIMENSION][3];
TreeOctNode::Neighbors3& neighbors=neighborKey.setNeighbors( node );
double width;
Point3D<Real> center;
Real w;
node->centerAndWidth(center,w);
width=w;
for( int i=0 ; i<DIMENSION ; i++ )
{
x = ( center[i] - position[i] - width ) / width;
dx[i][0] = 1.125 + 1.500*x + 0.500*x*x;
x = ( center[i] - position[i] ) / width;
dx[i][1] = 0.750 - x*x;
dx[i][2] = 1.0 - dx[i][1] - dx[i][0];
}
for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ )
{
dxdy = dx[0][i] * dx[1][j];
for( int k=0 ; k<3 ; k++ ) if( neighbors.neighbors[i][j][k] )
weight += Real( dxdy * dx[2][k] * neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution );
}
return Real( 1.0 / weight );
}
template<int Degree>
int Octree<Degree>::NonLinearUpdateWeightContribution( TreeOctNode* node , const Point3D<Real>& position , Real weight )
{
TreeOctNode::Neighbors3& neighbors = neighborKey.setNeighbors( node );
double x,dxdy,dx[DIMENSION][3];
double width;
Point3D<Real> center;
Real w;
node->centerAndWidth( center , w );
width=w;
constexpr double SAMPLE_SCALE = 1. / ( 0.125 * 0.125 + 0.75 * 0.75 + 0.125 * 0.125 );
for( int i=0 ; i<DIMENSION ; i++ )
{
x = ( center[i] - position[i] - width ) / width;
dx[i][0] = 1.125 + 1.500*x + 0.500*x*x;
x = ( center[i] - position[i] ) / width;
dx[i][1] = 0.750 - x*x;
dx[i][2] = 1. - dx[i][1] - dx[i][0];
// Note that we are splatting along a co-dimension one manifold, so uniform point samples
// do not generate a unit sample weight.
dx[i][0] *= SAMPLE_SCALE;
}
for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ )
{
dxdy = dx[0][i] * dx[1][j] * weight;
for( int k=0 ; k<3 ; k++ ) if( neighbors.neighbors[i][j][k] )
neighbors.neighbors[i][j][k]->nodeData.centerWeightContribution += Real( dxdy * dx[2][k] );
}
return 0;
}
template< int Degree > template<typename PointNT> int
Octree<Degree>::setTree(typename pcl::PointCloud<PointNT>::ConstPtr input_, int maxDepth , int minDepth ,
int kernelDepth , Real samplesPerNode , Real scaleFactor , Point3D<Real>& center , Real& scale ,
int useConfidence , Real constraintWeight , bool adaptiveWeights )
{
_minDepth = std::min< int >( std::max< int >( 0 , minDepth ) , maxDepth );
_constrainValues = (constraintWeight>0);
double pointWeightSum = 0;
Point3D<Real> min , max , position , normal , myCenter;
Real myWidth;
int i , cnt=0;
TreeOctNode* temp;
int splatDepth=0;
TreeNodeData::UseIndex = 1;
neighborKey.set( maxDepth );
splatDepth = kernelDepth;
if( splatDepth<0 ) splatDepth = 0;
tree.setFullDepth( _minDepth );
// Read through once to get the center and scale
while (cnt != input_->size ())
{
Point3D< Real > p;
p[0] = (*input_)[cnt].x;
p[1] = (*input_)[cnt].y;
p[2] = (*input_)[cnt].z;
for (i = 0; i < DIMENSION; i++)
{
if( !cnt || p[i]<min[i] ) min[i] = p[i];
if( !cnt || p[i]>max[i] ) max[i] = p[i];
}
cnt++;
}
scale = std::max< Real >( max[0]-min[0] , std::max< Real >( max[1]-min[1] , max[2]-min[2] ) );
center = ( max+min ) /2;
scale *= scaleFactor;
for( i=0 ; i<DIMENSION ; i++ ) center[i] -= scale/2;
if( splatDepth>0 )
{
cnt = 0;
while (cnt != input_->size ())
{
position[0] = (*input_)[cnt].x;
position[1] = (*input_)[cnt].y;
position[2] = (*input_)[cnt].z;
normal[0] = (*input_)[cnt].normal_x;
normal[1] = (*input_)[cnt].normal_y;
normal[2] = (*input_)[cnt].normal_z;
cnt++;
for( i=0 ; i<DIMENSION ; i++ ) position[i] = ( position[i]-center[i] ) / scale;
myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5);
myWidth = Real(1.0);
for( i=0 ; i<DIMENSION ; i++ ) if( position[i]<myCenter[i]-myWidth/2 || position[i]>myCenter[i]+myWidth/2 ) break;
if( i!=DIMENSION ) continue;
Real weight=Real( 1. );
if( useConfidence ) weight = Real( Length(normal) );
temp = &tree;
int d=0;
while( d<splatDepth )
{
NonLinearUpdateWeightContribution( temp , position , weight );
if( !temp->children ) temp->initChildren();
int cIndex=TreeOctNode::CornerIndex(myCenter,position);
temp=&temp->children[cIndex];
myWidth/=2;
if(cIndex&1) myCenter[0] += myWidth/2;
else myCenter[0] -= myWidth/2;
if(cIndex&2) myCenter[1] += myWidth/2;
else myCenter[1] -= myWidth/2;
if(cIndex&4) myCenter[2] += myWidth/2;
else myCenter[2] -= myWidth/2;
d++;
}
NonLinearUpdateWeightContribution( temp , position , weight );
}
}
normals = new std::vector< Point3D<Real> >();
cnt=0;
while (cnt != input_->size ())
{
position[0] = (*input_)[cnt].x;
position[1] = (*input_)[cnt].y;
position[2] = (*input_)[cnt].z;
normal[0] = (*input_)[cnt].normal_x;
normal[1] = (*input_)[cnt].normal_y;
normal[2] = (*input_)[cnt].normal_z;
cnt ++;
for( i=0 ; i<DIMENSION ; i++ ) position[i] = ( position[i]-center[i] ) / scale;
myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5);
myWidth = Real(1.0);
for( i=0 ; i<DIMENSION ; i++ ) if(position[i]<myCenter[i]-myWidth/2 || position[i]>myCenter[i]+myWidth/2) break;
if( i!=DIMENSION ) continue;
Real l = Real( Length( normal ) );
if( l!=l || l<=EPSILON ) continue;
if( !useConfidence ) normal /= l;
l = Real(1.);
Real pointWeight = Real(1.f);
if( samplesPerNode>0 && splatDepth )
{
pointWeight = NonLinearSplatOrientedPoint( position , normal , splatDepth , samplesPerNode , _minDepth , maxDepth );
}
else
{
Real alpha=1;
temp = &tree;
int d=0;
if( splatDepth )
{
while( d<splatDepth )
{
int cIndex=TreeOctNode::CornerIndex(myCenter,position);
temp=&temp->children[cIndex];
myWidth/=2;
if(cIndex&1) myCenter[0]+=myWidth/2;
else myCenter[0]-=myWidth/2;
if(cIndex&2) myCenter[1]+=myWidth/2;
else myCenter[1]-=myWidth/2;
if(cIndex&4) myCenter[2]+=myWidth/2;
else myCenter[2]-=myWidth/2;
d++;
}
alpha = NonLinearGetSampleWeight( temp , position );
}
for( i=0 ; i<DIMENSION ; i++ ) normal[i]*=alpha;
while( d<maxDepth )
{
if(!temp->children){temp->initChildren();}
int cIndex=TreeOctNode::CornerIndex(myCenter,position);
temp=&temp->children[cIndex];
myWidth/=2;
if(cIndex&1) myCenter[0]+=myWidth/2;
else myCenter[0]-=myWidth/2;
if(cIndex&2) myCenter[1]+=myWidth/2;
else myCenter[1]-=myWidth/2;
if(cIndex&4) myCenter[2]+=myWidth/2;
else myCenter[2]-=myWidth/2;
d++;
}
NonLinearSplatOrientedPoint( temp , position , normal );
pointWeight = alpha;
}
pointWeight = 1;
pointWeightSum += pointWeight;
if( _constrainValues )
{
int d = 0;
TreeOctNode* temp = &tree;
myCenter[0] = myCenter[1] = myCenter[2] = Real(0.5);
myWidth = Real(1.0);
while( 1 )
{
int idx = temp->nodeData.pointIndex;
if( idx==-1 )
{
Point3D< Real > p;
p[0] = p[1] = p[2] = 0;
idx = int( _points.size() );
_points.push_back( PointData( position*pointWeight , pointWeight ) );
temp->nodeData.pointIndex = idx;
}
else
{
_points[idx].weight += pointWeight;
_points[idx].position += position * pointWeight;
}
int cIndex = TreeOctNode::CornerIndex( myCenter , position );
if( !temp->children ) break;
temp = &temp->children[cIndex];
myWidth /= 2;
if( cIndex&1 ) myCenter[0] += myWidth/2;
else myCenter[0] -= myWidth/2;
if( cIndex&2 ) myCenter[1] += myWidth/2;
else myCenter[1] -= myWidth/2;
if( cIndex&4 ) myCenter[2] += myWidth/2;
else myCenter[2] -= myWidth/2;
d++;
}
}
}
if( _constrainValues )
for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode(n) )
if( n->nodeData.pointIndex!=-1 )
{
int idx = n->nodeData.pointIndex;
_points[idx].position /= _points[idx].weight;
if( adaptiveWeights ) _points[idx].weight *= (1<<n->d);
else _points[idx].weight *= (1<<maxDepth);
_points[idx].weight *= Real( constraintWeight / pointWeightSum );
}
#if FORCE_NEUMANN_FIELD
for( TreeOctNode* node=tree.nextNode() ; node ; node=tree.nextNode( node ) )
{
int d , off[3] , res;
node->depthAndOffset( d , off );
res = 1<<d;
if( node->nodeData.normalIndex<0 ) continue;
Point3D< Real >& normal = (*normals)[node->nodeData.normalIndex];
for( int d=0 ; d<3 ; d++ ) if( off[d]==0 || off[d]==res-1 ) normal[d] = 0;
}
#endif // FORCE_NEUMANN_FIELD
_sNodes.set( tree );
return cnt;
}
template<int Degree>
void Octree<Degree>::setBSplineData( int maxDepth , Real normalSmooth , bool reflectBoundary )
{
radius = 0.5 + 0.5 * Degree;
width=int(double(radius+0.5-EPSILON)*2);
if( normalSmooth>0 ) postNormalSmooth = normalSmooth;
fData.set( maxDepth , true , reflectBoundary );
}
template<int Degree>
void Octree<Degree>::finalize( void )
{
int maxDepth = tree.maxDepth( );
TreeOctNode::NeighborKey5 nKey;
nKey.set( maxDepth );
for( int d=maxDepth ; d>0 ; d-- )
for( TreeOctNode* node=tree.nextNode() ; node ; node=tree.nextNode( node ) )
if( node->d==d )
{
int xStart=0 , xEnd=5 , yStart=0 , yEnd=5 , zStart=0 , zEnd=5;
int c = int( node - node->parent->children );
int x , y , z;
Cube::FactorCornerIndex( c , x , y , z );
if( x ) xStart = 1;
else xEnd = 4;
if( y ) yStart = 1;
else yEnd = 4;
if( z ) zStart = 1;
else zEnd = 4;
nKey.setNeighbors( node->parent , xStart , xEnd , yStart , yEnd , zStart , zEnd );
}
_sNodes.set( tree );
MemoryUsage();
}
template< int Degree >
Real Octree< Degree >::GetValue( const PointInfo points[3][3][3] , const bool hasPoints[3][3] , const int d[3] ) const
{
double v = 0.;
const int min[] = { std::max<int>( 0 , d[0]+0 ) , std::max<int>( 0 , d[1]+0 ) , std::max<int>( 0 , d[2]+0 ) };
const int max[] = { std::min<int>( 2 , d[0]+2 ) , std::min<int>( 2 , d[1]+2 ) , std::min<int>( 2 , d[2]+2 ) };
for( int i=min[0] ; i<=max[0] ; i++ ) for( int j=min[1] ; j<=max[1] ; j++ )
{
if( !hasPoints[i][j] ) continue;
const PointInfo* pInfo = points[i][j];
int ii = -d[0]+i;
int jj = -d[1]+j;
for( int k=min[2] ; k<=max[2] ; k++ )
if( pInfo[k].weightedValue )
v += pInfo[k].splineValues[0][ii] * pInfo[k].splineValues[1][jj] * pInfo[k].splineValues[2][-d[2]+k];
}
return Real( v );
}
template<int Degree>
Real Octree<Degree>::GetLaplacian( const int idx[DIMENSION] ) const
{
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]] ) );
}
template< int Degree >
Real Octree< Degree >::GetLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 ) const
{
int symIndex[] =
{
BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) ,
BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) ,
BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] )
};
return GetLaplacian( symIndex );
}
template< int Degree >
Real Octree< Degree >::GetDivergence( const TreeOctNode* node1 , const TreeOctNode* node2 , const Point3D< Real >& normal1 ) const
{
int symIndex[] =
{
BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) ,
BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) ,
BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] ) ,
};
int aSymIndex[] =
{
#if GRADIENT_DOMAIN_SOLUTION
// Take the dot-product of the vector-field with the gradient of the basis function
fData.Index( node2->off[0] , node1->off[0] ) ,
fData.Index( node2->off[1] , node1->off[1] ) ,
fData.Index( node2->off[2] , node1->off[2] )
#else // !GRADIENT_DOMAIN_SOLUTION
// Take the dot-product of the divergence of the vector-field with the basis function
fData.Index( node1->off[0] , node2->off[0] ) ,
fData.Index( node1->off[1] , node2->off[1] ) ,
fData.Index( node1->off[2] , node2->off[2] )
#endif // GRADIENT_DOMAIN_SOLUTION
};
double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
#if GRADIENT_DOMAIN_SOLUTION
return Real( dot * ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) );
#else // !GRADIENT_DOMAIN_SOLUTION
return -Real( dot * ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] ) );
#endif // GRADIENT_DOMAIN_SOLUTION
}
template< int Degree >
Real Octree< Degree >::GetDivergenceMinusLaplacian( const TreeOctNode* node1 , const TreeOctNode* node2 , Real value1 , const Point3D<Real>& normal1 ) const
{
int symIndex[] =
{
BSplineData< Degree , Real >::SymmetricIndex( node1->off[0] , node2->off[0] ) ,
BSplineData< Degree , Real >::SymmetricIndex( node1->off[1] , node2->off[1] ) ,
BSplineData< Degree , Real >::SymmetricIndex( node1->off[2] , node2->off[2] )
};
int aSymIndex[] =
{
#if GRADIENT_DOMAIN_SOLUTION
// Take the dot-product of the vector-field with the gradient of the basis function
fData.Index( node2->off[0] , node1->off[0] ) ,
fData.Index( node2->off[1] , node1->off[1] ) ,
fData.Index( node2->off[2] , node1->off[2] )
#else // !GRADIENT_DOMAIN_SOLUTION
// Take the dot-product of the divergence of the vector-field with the basis function
fData.Index( node1->off[0] , node2->off[0] ) ,
fData.Index( node1->off[1] , node2->off[1] ) ,
fData.Index( node1->off[2] , node2->off[2] )
#endif // GRADIENT_DOMAIN_SOLUTION
};
double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
return Real( dot *
(
#if GRADIENT_DOMAIN_SOLUTION
- ( fData.ddDotTable[ symIndex[0]] + fData.ddDotTable[ symIndex[1]] + fData.ddDotTable[ symIndex[2]] ) * value1
+ ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] )
#else // !GRADIENT_DOMAIN_SOLUTION
- ( fData.ddDotTable[ symIndex[0]] + fData.ddDotTable[ symIndex[1]] + fData.ddDotTable[ symIndex[2]] ) * value1
- ( fData.dvDotTable[aSymIndex[0]]*normal1[0] + fData.dvDotTable[aSymIndex[1]]*normal1[1] + fData.dvDotTable[aSymIndex[2]]*normal1[2] )
#endif // GRADIENT_DOMAIN_SOLUTION
)
);
}
template< int Degree >
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
{
xStart = 0 , yStart = 0 , zStart = 0 , xEnd = 5 , yEnd = 5 , zEnd = 5;
int depth , off[3];
node->depthAndOffset( depth , off );
int width = 1 << ( depth-rDepth );
off[0] -= rOff[0] << ( depth-rDepth ) , off[1] -= rOff[1] << ( depth-rDepth ) , off[2] -= rOff[2] << ( depth-rDepth );
if( off[0]<0 ) xStart = -off[0];
if( off[1]<0 ) yStart = -off[1];
if( off[2]<0 ) zStart = -off[2];
if( off[0]>=width ) xEnd = 4 - ( off[0]-width );
if( off[1]>=width ) yEnd = 4 - ( off[1]-width );
if( off[2]>=width ) zEnd = 4 - ( off[2]-width );
}
template< int Degree >
int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 ) const { return GetMatrixRowSize( neighbors5 , 0 , 5 , 0 , 5 , 0 , 5 ); }
template< int Degree >
int Octree< Degree >::GetMatrixRowSize( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , int xStart , int xEnd , int yStart , int yEnd , int zStart , int zEnd ) const
{
int count = 0;
for( int x=xStart ; x<=2 ; x++ )
for( int y=yStart ; y<yEnd ; y++ )
if( x==2 && y>2 ) continue;
else for( int z=zStart ; z<zEnd ; z++ )
if( x==2 && y==2 && z>2 ) continue;
else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 )
count++;
return count;
}
template< int Degree >
int Octree< Degree >::SetMatrixRow( const OctNode< TreeNodeData , Real >::Neighbors5& neighbors5 , MatrixEntry< float >* row , int offset , const double stencil[5][5][5] ) const
{
return SetMatrixRow( neighbors5 , row , offset , stencil , 0 , 5 , 0 , 5 , 0 , 5 );
}
template< int Degree >
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
{
bool hasPoints[3][3];
Real diagonal = 0;
PointInfo samples[3][3][3];
int count = 0;
const TreeOctNode* node = neighbors5.neighbors[2][2][2];
int index[] = { int( node->off[0] ) , int( node->off[1] ), int( node->off[2] ) };
if( _constrainValues )
{
int d , idx[3];
node->depthAndOffset( d , idx );
idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] );
idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] );
idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] );
for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ )
{
hasPoints[j][k] = false;
for( int l=0 ; l<3 ; l++ )
{
const TreeOctNode* _node = neighbors5.neighbors[j+1][k+1][l+1];
if( _node && _node->nodeData.pointIndex!=-1 )
{
const PointData& pData = _points[ _node->nodeData.pointIndex ];
PointInfo& pointInfo = samples[j][k][l];
Real weight = pData.weight;
Point3D< Real > p = pData.position;
for( int s=0 ; s<3 ; s++ )
{
pointInfo.splineValues[0][s] = float( fData.baseBSplines[ idx[0]+j-s][s]( p[0] ) );
pointInfo.splineValues[1][s] = float( fData.baseBSplines[ idx[1]+k-s][s]( p[1] ) );
pointInfo.splineValues[2][s] = float( fData.baseBSplines[ idx[2]+l-s][s]( p[2] ) );
}
float value = pointInfo.splineValues[0][j] * pointInfo.splineValues[1][k] * pointInfo.splineValues[2][l];
diagonal += value * value * weight;
pointInfo.weightedValue = value * weight;
for( int s=0 ; s<3 ; s++ ) pointInfo.splineValues[0][s] *= pointInfo.weightedValue;
hasPoints[j][k] = true;
}
else samples[j][k][l].weightedValue = 0;
}
}
}
bool isInterior;
int d , off[3];
neighbors5.neighbors[2][2][2]->depthAndOffset( d , off );
int mn = 2 , mx = (1<<d)-2;
isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
for( int x=xStart ; x<=2 ; x++ )
for( int y=yStart ; y<yEnd ; y++ )
if( x==2 && y>2 ) continue;
else for( int z=zStart ; z<zEnd ; z++ )
if( x==2 && y==2 && z>2 ) continue;
else if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 )
{
const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
int _index[] = { int( _node->off[0] ) , int( _node->off[1] ), int( _node->off[2] ) };
Real temp;
if( isInterior ) temp = Real( stencil[x][y][z] );
else temp = GetLaplacian( node , _node );
if( _constrainValues )
{
int _d[] = { _index[0]-index[0] , _index[1]-index[1] , _index[2]-index[2] };
if( x==2 && y==2 && z==2 ) temp += diagonal;
else temp += GetValue( samples , hasPoints , _d );
}
if( x==2 && y==2 && z==2 ) temp /= 2;
if( fabs(temp)>MATRIX_ENTRY_EPSILON )
{
row[count].N = _node->nodeData.nodeIndex-offset;
row[count].Value = temp;
count++;
}
}
return count;
}
template< int Degree >
void Octree< Degree >::SetDivergenceStencil( int depth , Point3D< double > *stencil , bool scatter ) const
{
int offset[] = { 2 , 2 , 2 };
short d , off[3];
TreeOctNode::Index( depth , offset , d , off );
int index1[3] , index2[3];
if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
{
int _offset[] = { x , y , z };
TreeOctNode::Index( depth , _offset , d , off );
if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
int symIndex[] =
{
BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) ,
BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) ,
BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) ,
};
int aSymIndex[] =
{
#if GRADIENT_DOMAIN_SOLUTION
// Take the dot-product of the vector-field with the gradient of the basis function
fData.Index( index1[0] , index2[0] ) ,
fData.Index( index1[1] , index2[1] ) ,
fData.Index( index1[2] , index2[2] )
#else // !GRADIENT_DOMAIN_SOLUTION
// Take the dot-product of the divergence of the vector-field with the basis function
fData.Index( index2[0] , index1[0] ) ,
fData.Index( index2[1] , index1[1] ) ,
fData.Index( index2[2] , index1[2] )
#endif // GRADIENT_DOMAIN_SOLUTION
};
double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
#if GRADIENT_DOMAIN_SOLUTION
Point3D<double> temp;
temp[0] = fData.dvDotTable[aSymIndex[0]] * dot;
temp[1] = fData.dvDotTable[aSymIndex[1]] * dot;
temp[2] = fData.dvDotTable[aSymIndex[2]] * dot;
stencil[25*x + 5*y + z] = temp;
// stencil[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot;
// stencil[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot;
// stencil[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot;
#else // !GRADIENT_DOMAIN_SOLUTION
Point3D<double> temp;
temp[0] = -fData.dvDotTable[aSymIndex[0]] * dot;
temp[1] = -fData.dvDotTable[aSymIndex[1]] * dot;
temp[2] = -fData.dvDotTable[aSymIndex[2]] * dot;
stencil[25*x + 5*y + z] = temp;
// stencil[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot;
// stencil[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot;
// stencil[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot;
#endif // GRADIENT_DOMAIN_SOLUTION
}
}
template< int Degree >
void Octree< Degree >::SetLaplacianStencil( int depth , double stencil[5][5][5] ) const
{
int offset[] = { 2 , 2 , 2 };
short d , off[3];
TreeOctNode::Index( depth , offset , d , off );
int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) };
for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
{
int _offset[] = { x , y , z };
short _d , _off[3];
TreeOctNode::Index( depth , _offset , _d , _off );
int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) };
int symIndex[3];
symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] );
symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] );
symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] );
stencil[x][y][z] = GetLaplacian( symIndex );
}
}
template< int Degree >
void Octree< Degree >::SetLaplacianStencils( int depth , Stencil< double , 5 > stencils[2][2][2] ) const
{
if( depth<=1 ) return;
for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
{
short d , off[3];
int offset[] = { 4+i , 4+j , 4+k };
TreeOctNode::Index( depth , offset , d , off );
int index[] = { int( off[0] ) , int( off[1] ) , int( off[2] ) };
for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
{
int _offset[] = { x , y , z };
short _d , _off[3];
TreeOctNode::Index( depth-1 , _offset , _d , _off );
int _index[] = { int( _off[0] ) , int( _off[1] ) , int( _off[2] ) };
int symIndex[3];
symIndex[0] = BSplineData< Degree , Real >::SymmetricIndex( index[0] , _index[0] );
symIndex[1] = BSplineData< Degree , Real >::SymmetricIndex( index[1] , _index[1] );
symIndex[2] = BSplineData< Degree , Real >::SymmetricIndex( index[2] , _index[2] );
stencils[i][j][k].values[x][y][z] = GetLaplacian( symIndex );
}
}
}
template< int Degree >
void Octree< Degree >::SetDivergenceStencils( int depth , Stencil< Point3D< double > , 5 > stencils[2][2][2] , bool scatter ) const
{
if( depth<=1 ) return;
int index1[3] , index2[3];
for( int i=0 ; i<2 ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
{
short d , off[3];
int offset[] = { 4+i , 4+j , 4+k };
TreeOctNode::Index( depth , offset , d , off );
if( scatter ) index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
else index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
for( int x=0 ; x<5 ; x++ ) for( int y=0 ; y<5 ; y++ ) for( int z=0 ; z<5 ; z++ )
{
int _offset[] = { x , y , z };
TreeOctNode::Index( depth-1 , _offset , d , off );
if( scatter ) index1[0] = int( off[0] ) , index1[1] = int( off[1] ) , index1[2] = int( off[2] );
else index2[0] = int( off[0] ) , index2[1] = int( off[1] ) , index2[2] = int( off[2] );
int symIndex[] =
{
BSplineData< Degree , Real >::SymmetricIndex( index1[0] , index2[0] ) ,
BSplineData< Degree , Real >::SymmetricIndex( index1[1] , index2[1] ) ,
BSplineData< Degree , Real >::SymmetricIndex( index1[2] , index2[2] ) ,
};
int aSymIndex[] =
{
#if GRADIENT_DOMAIN_SOLUTION
// Take the dot-product of the vector-field with the gradient of the basis function
fData.Index( index1[0] , index2[0] ) ,
fData.Index( index1[1] , index2[1] ) ,
fData.Index( index1[2] , index2[2] )
#else // !GRADIENT_DOMAIN_SOLUTION
// Take the dot-product of the divergence of the vector-field with the basis function
fData.Index( index2[0] , index1[0] ) ,
fData.Index( index2[1] , index1[1] ) ,
fData.Index( index2[2] , index1[2] )
#endif // GRADIENT_DOMAIN_SOLUTION
};
double dot = fData.vvDotTable[symIndex[0]] * fData.vvDotTable[symIndex[1]] * fData.vvDotTable[symIndex[2]];
#if GRADIENT_DOMAIN_SOLUTION
stencils[i][j][k].values[x][y][z][0] = fData.dvDotTable[aSymIndex[0]] * dot;
stencils[i][j][k].values[x][y][z][1] = fData.dvDotTable[aSymIndex[1]] * dot;
stencils[i][j][k].values[x][y][z][2] = fData.dvDotTable[aSymIndex[2]] * dot;
#else // !GRADIENT_DOMAIN_SOLUTION
stencils[i][j][k].values[x][y][z][0] = -fData.dvDotTable[aSymIndex[0]] * dot;
stencils[i][j][k].values[x][y][z][1] = -fData.dvDotTable[aSymIndex[1]] * dot;
stencils[i][j][k].values[x][y][z][2] = -fData.dvDotTable[aSymIndex[2]] * dot;
#endif // GRADIENT_DOMAIN_SOLUTION
}
}
}
template< int Degree >
void Octree< Degree >::SetEvaluationStencils( int depth , Stencil< double , 3 > stencil1[8] , Stencil< double , 3 > stencil2[8][8] ) const
{
if( depth>2 )
{
int idx[3];
int off[] = { 2 , 2 , 2 };
for( int c=0 ; c<8 ; c++ )
{
VertexData::CornerIndex( depth , off , c , fData.depth , idx );
idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount;
for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ )
{
short _d , _off[3];
int _offset[] = { x+1 , y+1 , z+1 };
TreeOctNode::Index( depth , _offset , _d , _off );
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]) ];
}
}
}
if( depth>3 )
for( int _c=0 ; _c<8 ; _c++ )
{
int idx[3];
int _cx , _cy , _cz;
Cube::FactorCornerIndex( _c , _cx , _cy , _cz );
int off[] = { 4+_cx , 4+_cy , 4+_cz };
for( int c=0 ; c<8 ; c++ )
{
VertexData::CornerIndex( depth , off , c , fData.depth , idx );
idx[0] *= fData.functionCount , idx[1] *= fData.functionCount , idx[2] *= fData.functionCount;
for( int x=0 ; x<3 ; x++ ) for( int y=0 ; y<3 ; y++ ) for( int z=0 ; z<3 ; z++ )
{
short _d , _off[3];
int _offset[] = { x+1 , y+1 , z+1 };
TreeOctNode::Index( depth-1 , _offset , _d , _off );
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]) ];
}
}
}
}
template< int Degree >
void Octree< Degree >::UpdateCoarserSupportBounds( const TreeOctNode* node , int& startX , int& endX , int& startY , int& endY , int& startZ , int& endZ )
{
if( node->parent )
{
int x , y , z , c = int( node - node->parent->children );
Cube::FactorCornerIndex( c , x , y , z );
if( x==0 ) endX = 4;
else startX = 1;
if( y==0 ) endY = 4;
else startY = 1;
if( z==0 ) endZ = 4;
else startZ = 1;
}
}
template< int Degree >
void Octree< Degree >::UpdateConstraintsFromCoarser( const OctNode< TreeNodeData , Real >::NeighborKey5& neighborKey5 , TreeOctNode* node , Real* metSolution , const Stencil< double , 5 >& lapStencil ) const
{
bool isInterior;
{
int d , off[3];
node->depthAndOffset( d , off );
int mn = 4 , mx = (1<<d)-4;
isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
}
Real constraint = Real( 0 );
int depth = node->depth();
if( depth<=_minDepth ) return;
int i = node->nodeData.nodeIndex;
// Offset the constraints using the solution from lower resolutions.
int startX = 0 , endX = 5 , startY = 0 , endY = 5 , startZ = 0 , endZ = 5;
UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ );
const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth-1];
for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.nodeIndex>=0 )
{
const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
Real _solution = metSolution[ _node->nodeData.nodeIndex ];
{
if( isInterior ) node->nodeData.constraint -= Real( lapStencil.values[x][y][z] * _solution );
else node->nodeData.constraint -= GetLaplacian( _node , node ) * _solution;
}
}
if( _constrainValues )
{
int d , idx[3];
node->depthAndOffset( d, idx );
idx[0] = BinaryNode< double >::CenterIndex( d , idx[0] );
idx[1] = BinaryNode< double >::CenterIndex( d , idx[1] );
idx[2] = BinaryNode< double >::CenterIndex( d , idx[2] );
const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth];
for( int x=1 ; x<4 ; x++ ) for( int y=1 ; y<4 ; y++ ) for( int z=1 ; z<4 ; z++ )
if( neighbors5.neighbors[x][y][z] && neighbors5.neighbors[x][y][z]->nodeData.pointIndex!=-1 )
{
const PointData& pData = _points[ neighbors5.neighbors[x][y][z]->nodeData.pointIndex ];
Real pointValue = pData.value;
Point3D< Real > p = pData.position;
node->nodeData.constraint -=
Real(
fData.baseBSplines[idx[0]][x-1]( p[0] ) *
fData.baseBSplines[idx[1]][y-1]( p[1] ) *
fData.baseBSplines[idx[2]][z-1]( p[2] ) *
pointValue
);
}
}
}
struct UpSampleData
{
int start;
double v[2];
UpSampleData( void ) { start = 0 , v[0] = v[1] = 0.; }
UpSampleData( int s , double v1 , double v2 ) { start = s , v[0] = v1 , v[1] = v2; }
};
template< int Degree >
void Octree< Degree >::UpSampleCoarserSolution( int depth , const SortedTreeNodes& sNodes , Vector< Real >& Solution ) const
{
int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
Solution.Resize( range );
if( !depth ) return;
else if( depth==1 ) for( int i=start ; i<end ; i++ ) Solution[i-start] += sNodes.treeNodes[0]->nodeData.solution;
else
{
// For every node at the current depth
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::NeighborKey3 neighborKey;
neighborKey.set( depth );
for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
{
int d , off[3];
UpSampleData usData[3];
sNodes.treeNodes[i]->depthAndOffset( d , off );
for( int d=0 ; d<3 ; d++ )
{
if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
}
neighborKey.getNeighbors( sNodes.treeNodes[i] );
for( int ii=0 ; ii<2 ; ii++ )
{
int _ii = ii + usData[0].start;
double dx = usData[0].v[ii];
for( int jj=0 ; jj<2 ; jj++ )
{
int _jj = jj + usData[1].start;
double dxy = dx * usData[1].v[jj];
for( int kk=0 ; kk<2 ; kk++ )
{
int _kk = kk + usData[2].start;
double dxyz = dxy * usData[2].v[kk];
Solution[i-start] += Real( neighborKey.neighbors[depth-1].neighbors[_ii][_jj][_kk]->nodeData.solution * dxyz );
}
}
}
}
}
}
// Clear the coarser solution
start = sNodes.nodeCount[depth-1] , end = sNodes.nodeCount[depth] , range = end-start;
#pragma omp parallel for num_threads( threads )
for( int i=start ; i<end ; i++ ) sNodes.treeNodes[i]->nodeData.solution = Real( 0. );
}
template< int Degree >
void Octree< Degree >::DownSampleFinerConstraints( int depth , SortedTreeNodes& sNodes ) const
{
if( !depth ) return;
#pragma omp parallel for num_threads( threads )
for( int i=sNodes.nodeCount[depth-1] ; i<sNodes.nodeCount[depth] ; i++ )
sNodes.treeNodes[i]->nodeData.constraint = Real( 0 );
if( depth==1 )
{
sNodes.treeNodes[0]->nodeData.constraint = Real( 0 );
for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) sNodes.treeNodes[0]->nodeData.constraint += sNodes.treeNodes[i]->nodeData.constraint;
return;
}
std::vector< Vector< double > > constraints( threads );
for( int t=0 ; t<threads ; t++ ) constraints[t].Resize( sNodes.nodeCount[depth] - sNodes.nodeCount[depth-1] ) , constraints[t].SetZero();
int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
int lStart = sNodes.nodeCount[depth-1] , lEnd = sNodes.nodeCount[depth];
// For every node at the current depth
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::NeighborKey3 neighborKey;
neighborKey.set( depth );
for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
{
int d , off[3];
UpSampleData usData[3];
sNodes.treeNodes[i]->depthAndOffset( d , off );
for( int d=0 ; d<3 ; d++ )
{
if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
}
neighborKey.getNeighbors( sNodes.treeNodes[i] );
TreeOctNode::Neighbors3& neighbors = neighborKey.neighbors[depth-1];
for( int ii=0 ; ii<2 ; ii++ )
{
int _ii = ii + usData[0].start;
double dx = usData[0].v[ii];
for( int jj=0 ; jj<2 ; jj++ )
{
int _jj = jj + usData[1].start;
double dxy = dx * usData[1].v[jj];
for( int kk=0 ; kk<2 ; kk++ )
{
int _kk = kk + usData[2].start;
double dxyz = dxy * usData[2].v[kk];
constraints[t][neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex-lStart] += sNodes.treeNodes[i]->nodeData.constraint * dxyz;
}
}
}
}
}
#pragma omp parallel for num_threads( threads )
for( int i=lStart ; i<lEnd ; i++ )
{
Real cSum = Real(0.);
for( int t=0 ; t<threads ; t++ ) cSum += constraints[t][i-lStart];
sNodes.treeNodes[i]->nodeData.constraint += cSum;
}
}
template< int Degree >
template< class C >
void Octree< Degree >::DownSample( int depth , const SortedTreeNodes& sNodes , C* constraints ) const
{
if( depth==0 ) return;
if( depth==1 )
{
for( int i=sNodes.nodeCount[1] ; i<sNodes.nodeCount[2] ; i++ ) constraints[0] += constraints[i];
return;
}
std::vector< Vector< C > > _constraints( threads );
for( int t=0 ; t<threads ; t++ ) _constraints[t].Resize( sNodes.nodeCount[depth] - sNodes.nodeCount[depth-1] );
int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start , lStart = sNodes.nodeCount[depth-1] , lEnd = sNodes.nodeCount[depth];
// For every node at the current depth
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::NeighborKey3 neighborKey;
neighborKey.set( depth );
for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
{
int d , off[3];
UpSampleData usData[3];
sNodes.treeNodes[i]->depthAndOffset( d , off );
for( int d=0 ; d<3 ; d++ )
{
if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
}
TreeOctNode::Neighbors3& neighbors = neighborKey.getNeighbors( sNodes.treeNodes[i]->parent );
C c = constraints[i];
for( int ii=0 ; ii<2 ; ii++ )
{
int _ii = ii + usData[0].start;
C cx = C( c*usData[0].v[ii] );
for( int jj=0 ; jj<2 ; jj++ )
{
int _jj = jj + usData[1].start;
C cxy = C( cx*usData[1].v[jj] );
for( int kk=0 ; kk<2 ; kk++ )
{
int _kk = kk + usData[2].start;
if( neighbors.neighbors[_ii][_jj][_kk] )
_constraints[t][neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex-lStart] += C( cxy*usData[2].v[kk] );
}
}
}
}
}
#pragma omp parallel for num_threads( threads )
for( int i=lStart ; i<lEnd ; i++ )
{
C cSum = C(0);
for( int t=0 ; t<threads ; t++ ) cSum += _constraints[t][i-lStart];
constraints[i] += cSum;
}
}
template< int Degree >
template< class C >
void Octree< Degree >::UpSample( int depth , const SortedTreeNodes& sNodes , C* coefficients ) const
{
if ( depth==0 ) return;
else if( depth==1 )
{
for( int i=sNodes.nodeCount[1] ; i<sNodes.nodeCount[2] ; i++ ) coefficients[i] += coefficients[0];
return;
}
int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
// For every node at the current depth
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::NeighborKey3 neighborKey;
neighborKey.set( depth-1 );
for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
{
TreeOctNode* node = sNodes.treeNodes[i];
int d , off[3];
UpSampleData usData[3];
node->depthAndOffset( d , off );
for( int d=0 ; d<3 ; d++ )
{
if ( off[d] ==0 ) usData[d] = UpSampleData( 1 , 1.00 , 0.00 );
else if( off[d]+1==(1<<depth) ) usData[d] = UpSampleData( 0 , 0.00 , 1.00 );
else if( off[d]%2 ) usData[d] = UpSampleData( 1 , 0.75 , 0.25 );
else usData[d] = UpSampleData( 0 , 0.25 , 0.75 );
}
TreeOctNode::Neighbors3& neighbors = neighborKey.getNeighbors( node->parent );
for( int ii=0 ; ii<2 ; ii++ )
{
int _ii = ii + usData[0].start;
double dx = usData[0].v[ii];
for( int jj=0 ; jj<2 ; jj++ )
{
int _jj = jj + usData[1].start;
double dxy = dx * usData[1].v[jj];
for( int kk=0 ; kk<2 ; kk++ )
{
int _kk = kk + usData[2].start;
if( neighbors.neighbors[_ii][_jj][_kk] )
{
double dxyz = dxy * usData[2].v[kk];
int _i = neighbors.neighbors[_ii][_jj][_kk]->nodeData.nodeIndex;
coefficients[i] += coefficients[_i] * Real( dxyz );
}
}
}
}
}
}
}
template< int Degree >
void Octree< Degree >::SetCoarserPointValues( int depth , const SortedTreeNodes& sNodes , Real* metSolution )
{
int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
// For every node at the current depth
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::NeighborKey3 neighborKey;
neighborKey.set( depth );
for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
{
int pIdx = sNodes.treeNodes[i]->nodeData.pointIndex;
if( pIdx!=-1 )
{
neighborKey.getNeighbors( sNodes.treeNodes[i] );
_points[ pIdx ].value = WeightedCoarserFunctionValue( neighborKey , sNodes.treeNodes[i] , metSolution );
}
}
}
}
template< int Degree >
Real Octree< Degree >::WeightedCoarserFunctionValue( const OctNode< TreeNodeData , Real >::NeighborKey3& neighborKey , const TreeOctNode* pointNode , Real* metSolution ) const
{
int depth = pointNode->depth();
if( !depth || pointNode->nodeData.pointIndex==-1 ) return Real(0.);
double pointValue = 0;
Real weight = _points[ pointNode->nodeData.pointIndex ].weight;
Point3D< Real > p = _points[ pointNode->nodeData.pointIndex ].position;
// Iterate over all basis functions that overlap the point at the coarser resolutions
{
int d , _idx[3];
const TreeOctNode::Neighbors3& neighbors = neighborKey.neighbors[depth-1];
neighbors.neighbors[1][1][1]->depthAndOffset( d , _idx );
_idx[0] = BinaryNode< double >::CenterIndex( d , _idx[0]-1 );
_idx[1] = BinaryNode< double >::CenterIndex( d , _idx[1]-1 );
_idx[2] = BinaryNode< double >::CenterIndex( d , _idx[2]-1 );
for( int j=0 ; j<3 ; j++ ) for( int k=0 ; k<3 ; k++ ) for( int l=0 ; l<3 ; l++ )
if( neighbors.neighbors[j][k][l] && neighbors.neighbors[j][k][l]->nodeData.nodeIndex>=0 )
{
// Accumulate the contribution from these basis nodes
const TreeOctNode* basisNode = neighbors.neighbors[j][k][l];
int idx[] = { _idx[0]+j , _idx[1]+k , _idx[2]+l };
pointValue +=
fData.baseBSplines[ idx[0] ][2-j]( p[0] ) *
fData.baseBSplines[ idx[1] ][2-k]( p[1] ) *
fData.baseBSplines[ idx[2] ][2-l]( p[2] ) *
metSolution[basisNode->nodeData.nodeIndex];
}
}
return Real( pointValue * weight );
}
template<int Degree>
int Octree<Degree>::GetFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix , int depth , const SortedTreeNodes& sNodes , Real* metSolution )
{
int start = sNodes.nodeCount[depth] , end = sNodes.nodeCount[depth+1] , range = end-start;
double stencil[5][5][5];
SetLaplacianStencil( depth , stencil );
Stencil< double , 5 > stencils[2][2][2];
SetLaplacianStencils( depth , stencils );
matrix.Resize( range );
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::NeighborKey5 neighborKey5;
neighborKey5.set( depth );
for( int i=(range*t)/threads ; i<(range*(t+1))/threads ; i++ )
{
TreeOctNode* node = sNodes.treeNodes[i+start];
neighborKey5.getNeighbors( node );
// Get the matrix row size
int count = GetMatrixRowSize( neighborKey5.neighbors[depth] );
// Allocate memory for the row
#pragma omp critical (matrix_set_row_size)
{
matrix.SetRowSize( i , count );
}
// Set the row entries
matrix.rowSizes[i] = SetMatrixRow( neighborKey5.neighbors[depth] , matrix[i] , start , stencil );
// Offset the constraints using the solution from lower resolutions.
int x , y , z , c;
if( node->parent )
{
c = int( node - node->parent->children );
Cube::FactorCornerIndex( c , x , y , z );
}
else x = y = z = 0;
UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] );
}
}
return 1;
}
template<int Degree>
int Octree<Degree>::GetRestrictedFixedDepthLaplacian( SparseSymmetricMatrix< Real >& matrix,int depth,const int* entries,int entryCount,
const TreeOctNode* rNode , Real radius ,
const SortedTreeNodes& sNodes , Real* metSolution )
{
for( int i=0 ; i<entryCount ; i++ ) sNodes.treeNodes[ entries[i] ]->nodeData.nodeIndex = i;
double stencil[5][5][5];
int rDepth , rOff[3];
rNode->depthAndOffset( rDepth , rOff );
matrix.Resize( entryCount );
SetLaplacianStencil( depth , stencil );
Stencil< double , 5 > stencils[2][2][2];
SetLaplacianStencils( depth , stencils );
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::NeighborKey5 neighborKey5;
neighborKey5.set( depth );
for( int i=(entryCount*t)/threads ; i<(entryCount*(t+1))/threads ; i++ )
{
TreeOctNode* node = sNodes.treeNodes[ entries[i] ];
int d , off[3];
node->depthAndOffset( d , off );
off[0] >>= (depth-rDepth) , off[1] >>= (depth-rDepth) , off[2] >>= (depth-rDepth);
bool isInterior = ( off[0]==rOff[0] && off[1]==rOff[1] && off[2]==rOff[2] );
neighborKey5.getNeighbors( node );
int xStart=0 , xEnd=5 , yStart=0 , yEnd=5 , zStart=0 , zEnd=5;
if( !isInterior ) SetMatrixRowBounds( neighborKey5.neighbors[depth].neighbors[2][2][2] , rDepth , rOff , xStart , xEnd , yStart , yEnd , zStart , zEnd );
// Get the matrix row size
int count = GetMatrixRowSize( neighborKey5.neighbors[depth] , xStart , xEnd , yStart , yEnd , zStart , zEnd );
// Allocate memory for the row
#pragma omp critical (matrix_set_row_size)
{
matrix.SetRowSize( i , count );
}
// Set the matrix row entries
matrix.rowSizes[i] = SetMatrixRow( neighborKey5.neighbors[depth] , matrix[i] , 0 , stencil , xStart , xEnd , yStart , yEnd , zStart , zEnd );
// Adjust the system constraints
int x , y , z , c;
if( node->parent )
{
c = int( node - node->parent->children );
Cube::FactorCornerIndex( c , x , y , z );
}
else x = y = z = 0;
UpdateConstraintsFromCoarser( neighborKey5 , node , metSolution , stencils[x][y][z] );
}
}
for( int i=0 ; i<entryCount ; i++ ) sNodes.treeNodes[entries[i]]->nodeData.nodeIndex = entries[i];
return 1;
}
template<int Degree>
int Octree<Degree>::LaplacianMatrixIteration( int subdivideDepth , bool showResidual , int minIters , double accuracy )
{
int i,iter=0;
double t = 0;
fData.setDotTables( fData.DD_DOT_FLAG | fData.DV_DOT_FLAG );
SparseMatrix< float >::SetAllocator( MEMORY_ALLOCATOR_BLOCK_SIZE );
_sNodes.treeNodes[0]->nodeData.solution = 0;
std::vector< Real > metSolution( _sNodes.nodeCount[ _sNodes.maxDepth ] , 0 );
for( i=1 ; i<_sNodes.maxDepth ; i++ )
{
if( subdivideDepth>0 ) iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , subdivideDepth , showResidual , minIters , accuracy );
else iter += SolveFixedDepthMatrix( i , _sNodes , &metSolution[0] , showResidual , minIters , accuracy );
}
SparseMatrix< float >::internalAllocator.reset();
fData.clearDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG | fData.DD_DOT_FLAG );
return iter;
}
template<int Degree>
int Octree<Degree>::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , bool showResidual , int minIters , double accuracy )
{
int iter = 0;
Vector< Real > X , B;
SparseSymmetricMatrix< Real > M;
double systemTime=0. , solveTime=0. , updateTime=0. , evaluateTime = 0.;
X.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] );
if( depth<=_minDepth ) UpSampleCoarserSolution( depth , sNodes , X );
else
{
// Up-sample the cumulative solution into the previous depth
UpSample( depth-1 , sNodes , metSolution );
// Add in the solution from that depth
if( depth )
#pragma omp parallel for num_threads( threads )
for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution;
}
if( _constrainValues )
{
SetCoarserPointValues( depth , sNodes , metSolution );
}
SparseSymmetricMatrix< Real >::internalAllocator.rollBack();
{
int maxECount = ( (2*Degree+1)*(2*Degree+1)*(2*Degree+1) + 1 ) / 2;
maxECount = ( ( maxECount + 15 ) / 16 ) * 16;
M.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] );
for( int i=0 ; i<M.rows ; i++ ) M.SetRowSize( i , maxECount );
}
{
// Get the system matrix
SparseSymmetricMatrix< Real >::internalAllocator.rollBack();
GetFixedDepthLaplacian( M , depth , sNodes , metSolution );
// Set the constraint vector
B.Resize( sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth] );
for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) B[i-sNodes.nodeCount[depth]] = sNodes.treeNodes[i]->nodeData.constraint;
}
// Solve the linear system
iter += SparseSymmetricMatrix< Real >::Solve( M , B , std::max< int >( int( pow( M.rows , ITERATION_POWER ) ) , minIters ) , X , Real(accuracy) , 0 , threads , (depth<=_minDepth) && !_constrainValues );
if( showResidual )
{
double mNorm = 0;
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;
double bNorm = B.Norm( 2 ) , rNorm = ( B - M * X ).Norm( 2 );
printf( "\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter );
}
// Copy the solution back into the tree (over-writing the constraints)
for( int i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ ) sNodes.treeNodes[i]->nodeData.solution = Real( X[i-sNodes.nodeCount[depth]] );
return iter;
}
template<int Degree>
int Octree<Degree>::SolveFixedDepthMatrix( int depth , const SortedTreeNodes& sNodes , Real* metSolution , int startingDepth , bool showResidual , int minIters , double accuracy )
{
if( startingDepth>=depth ) return SolveFixedDepthMatrix( depth , sNodes , metSolution , showResidual , minIters , accuracy );
int i , j , d , tIter=0;
SparseSymmetricMatrix< Real > _M;
Vector< Real > B , B_ , X_;
AdjacencySetFunction asf;
AdjacencyCountFunction acf;
double systemTime = 0 , solveTime = 0 , memUsage = 0 , evaluateTime = 0 , gTime = 0, sTime = 0;
Real myRadius , myRadius2;
if( depth>_minDepth )
{
// Up-sample the cumulative solution into the previous depth
UpSample( depth-1 , sNodes , metSolution );
// Add in the solution from that depth
if( depth )
#pragma omp parallel for num_threads( threads )
for( int i=_sNodes.nodeCount[depth-1] ; i<_sNodes.nodeCount[depth] ; i++ ) metSolution[i] += _sNodes.treeNodes[i]->nodeData.solution;
}
if( _constrainValues )
{
SetCoarserPointValues( depth , sNodes , metSolution );
}
B.Resize( sNodes.nodeCount[depth+1] - sNodes.nodeCount[depth] );
// Back-up the constraints
for( i=sNodes.nodeCount[depth] ; i<sNodes.nodeCount[depth+1] ; i++ )
{
B[ i-sNodes.nodeCount[depth] ] = sNodes.treeNodes[i]->nodeData.constraint;
sNodes.treeNodes[i]->nodeData.constraint = 0;
}
myRadius = 2*radius-Real(0.5);
myRadius = int(myRadius-ROUND_EPS)+ROUND_EPS;
myRadius2 = Real(radius+ROUND_EPS-0.5);
d = depth-startingDepth;
std::vector< int > subDimension( sNodes.nodeCount[d+1]-sNodes.nodeCount[d] );
int maxDimension = 0;
for( i=sNodes.nodeCount[d] ; i<sNodes.nodeCount[d+1] ; i++ )
{
// Count the number of nodes at depth "depth" that lie under sNodes.treeNodes[i]
acf.adjacencyCount = 0;
for( TreeOctNode* temp=sNodes.treeNodes[i]->nextNode() ; temp ; )
{
if( temp->depth()==depth ) acf.adjacencyCount++ , temp = sNodes.treeNodes[i]->nextBranch( temp );
else temp = sNodes.treeNodes[i]->nextNode ( temp );
}
for( j=sNodes.nodeCount[d] ; j<sNodes.nodeCount[d+1] ; j++ )
{
if( i==j ) continue;
TreeOctNode::ProcessFixedDepthNodeAdjacentNodes( fData.depth , sNodes.treeNodes[i] , 1 , sNodes.treeNodes[j] , 2*width-1 , depth , &acf );
}
subDimension[i-sNodes.nodeCount[d]] = acf.adjacencyCount;
maxDimension = std::max< int >( maxDimension , subDimension[i-sNodes.nodeCount[d]] );
}
asf.adjacencies = new int[maxDimension];
MapReduceVector< Real > mrVector;
mrVector.resize( threads , maxDimension );
// Iterate through the coarse-level nodes
for( i=sNodes.nodeCount[d] ; i<sNodes.nodeCount[d+1] ; i++ )
{
int iter = 0;
// Count the number of nodes at depth "depth" that lie under sNodes.treeNodes[i]
acf.adjacencyCount = subDimension[i-sNodes.nodeCount[d]];
if( !acf.adjacencyCount ) continue;
// Set the indices for the nodes under, or near, sNodes.treeNodes[i].
asf.adjacencyCount = 0;
for( TreeOctNode* temp=sNodes.treeNodes[i]->nextNode() ; temp ; )
{
if( temp->depth()==depth ) asf.adjacencies[ asf.adjacencyCount++ ] = temp->nodeData.nodeIndex , temp = sNodes.treeNodes[i]->nextBranch( temp );
else temp = sNodes.treeNodes[i]->nextNode ( temp );
}
for( j=sNodes.nodeCount[d] ; j<sNodes.nodeCount[d+1] ; j++ )
{
if( i==j ) continue;
TreeOctNode::ProcessFixedDepthNodeAdjacentNodes( fData.depth , sNodes.treeNodes[i] , 1 , sNodes.treeNodes[j] , 2*width-1 , depth , &asf );
}
// Get the associated constraint vector
B_.Resize( asf.adjacencyCount );
for( j=0 ; j<asf.adjacencyCount ; j++ ) B_[j] = B[ asf.adjacencies[j]-sNodes.nodeCount[depth] ];
X_.Resize( asf.adjacencyCount );
#pragma omp parallel for num_threads( threads ) schedule( static )
for( j=0 ; j<asf.adjacencyCount ; j++ )
{
X_[j] = sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution;
}
// Get the associated matrix
SparseSymmetricMatrix< Real >::internalAllocator.rollBack();
GetRestrictedFixedDepthLaplacian( _M , depth , asf.adjacencies , asf.adjacencyCount , sNodes.treeNodes[i] , myRadius , sNodes , metSolution );
#pragma omp parallel for num_threads( threads ) schedule( static )
for( j=0 ; j<asf.adjacencyCount ; j++ )
{
B_[j] += sNodes.treeNodes[asf.adjacencies[j]]->nodeData.constraint;
sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.constraint = 0;
}
// Solve the matrix
// Since we don't have the full matrix, the system shouldn't be singular, so we shouldn't have to correct it
iter += SparseSymmetricMatrix< Real >::Solve( _M , B_ , std::max< int >( int( pow( _M.rows , ITERATION_POWER ) ) , minIters ) , X_ , mrVector , Real(accuracy) , 0 );
if( showResidual )
{
double mNorm = 0;
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;
double bNorm = B_.Norm( 2 ) , rNorm = ( B_ - _M * X_ ).Norm( 2 );
printf( "\t\tResidual: (%d %g) %g -> %g (%f) [%d]\n" , _M.Entries() , sqrt(mNorm) , bNorm , rNorm , rNorm/bNorm , iter );
}
// Update the solution for all nodes in the sub-tree
for( j=0 ; j<asf.adjacencyCount ; j++ )
{
TreeOctNode* temp=sNodes.treeNodes[ asf.adjacencies[j] ];
while( temp->depth()>sNodes.treeNodes[i]->depth() ) temp=temp->parent;
if( temp->nodeData.nodeIndex>=sNodes.treeNodes[i]->nodeData.nodeIndex ) sNodes.treeNodes[ asf.adjacencies[j] ]->nodeData.solution = Real( X_[j] );
}
systemTime += gTime;
solveTime += sTime;
memUsage = std::max< double >( MemoryUsage() , memUsage );
tIter += iter;
}
delete[] asf.adjacencies;
return tIter;
}
template<int Degree>
int Octree<Degree>::HasNormals(TreeOctNode* node,Real epsilon)
{
int hasNormals=0;
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;
if( node->children ) for(int i=0;i<Cube::CORNERS && !hasNormals;i++) hasNormals |= HasNormals(&node->children[i],epsilon);
return hasNormals;
}
template<int Degree>
void Octree<Degree>::ClipTree( void )
{
int maxDepth = tree.maxDepth();
for( TreeOctNode* temp=tree.nextNode() ; temp ; temp=tree.nextNode(temp) )
if( temp->children && temp->d>=_minDepth )
{
int hasNormals=0;
for( int i=0 ; i<Cube::CORNERS && !hasNormals ; i++ ) hasNormals = HasNormals( &temp->children[i] , EPSILON/(1<<maxDepth) );
if( !hasNormals ) temp->children=NULL;
}
MemoryUsage();
}
template<int Degree>
void Octree<Degree>::SetLaplacianConstraints( void )
{
// To set the Laplacian constraints, we iterate over the
// splatted normals and compute the dot-product of the
// divergence of the normal field with all the basis functions.
// Within the same depth: set directly as a gather
// Coarser depths
fData.setDotTables( fData.VV_DOT_FLAG | fData.DV_DOT_FLAG );
int maxDepth = _sNodes.maxDepth-1;
Point3D< Real > zeroPoint;
zeroPoint[0] = zeroPoint[1] = zeroPoint[2] = 0;
std::vector< Real > constraints( _sNodes.nodeCount[maxDepth] , Real(0) );
std::vector< Point3D< Real > > coefficients( _sNodes.nodeCount[maxDepth] , zeroPoint );
// Clear the constraints
#pragma omp parallel for num_threads( threads )
for( int i=0 ; i<_sNodes.nodeCount[maxDepth+1] ; i++ ) _sNodes.treeNodes[i]->nodeData.constraint = Real( 0. );
// For the scattering part of the operation, we parallelize by duplicating the constraints and then summing at the end.
std::vector< std::vector< Real > > _constraints( threads );
for( int t=0 ; t<threads ; t++ ) _constraints[t].resize( _sNodes.nodeCount[maxDepth] , 0 );
for( int d=maxDepth ; d>=0 ; d-- )
{
Point3D< double > stencil[5][5][5];
SetDivergenceStencil( d , &stencil[0][0][0] , false );
Stencil< Point3D< double > , 5 > stencils[2][2][2];
SetDivergenceStencils( d , stencils , true );
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::NeighborKey5 neighborKey5;
neighborKey5.set( fData.depth );
int start = _sNodes.nodeCount[d] , end = _sNodes.nodeCount[d+1] , range = end-start;
for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
{
TreeOctNode* node = _sNodes.treeNodes[i];
int startX=0 , endX=5 , startY=0 , endY=5 , startZ=0 , endZ=5;
int depth = node->depth();
neighborKey5.getNeighbors( node );
bool isInterior , isInterior2;
{
int d , off[3];
node->depthAndOffset( d , off );
int mn = 2 , mx = (1<<d)-2;
isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
mn += 2 , mx -= 2;
isInterior2 = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
}
int cx , cy , cz;
if( d )
{
int c = int( node - node->parent->children );
Cube::FactorCornerIndex( c , cx , cy , cz );
}
else cx = cy = cz = 0;
Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz];
// Set constraints from current depth
{
const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth];
if( isInterior )
for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
{
const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
if( _node && _node->nodeData.normalIndex>=0 )
{
const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex];
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] );
}
}
else
for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
{
const TreeOctNode* _node = neighbors5.neighbors[x][y][z];
if( _node && _node->nodeData.normalIndex>=0 )
{
const Point3D< Real >& _normal = (*normals)[_node->nodeData.normalIndex];
node->nodeData.constraint += GetDivergence( _node , node , _normal );
}
}
UpdateCoarserSupportBounds( neighbors5.neighbors[2][2][2] , startX , endX , startY , endY , startZ , endZ );
}
if( node->nodeData.nodeIndex<0 || node->nodeData.normalIndex<0 ) continue;
const Point3D< Real >& normal = (*normals)[node->nodeData.normalIndex];
if( normal[0]==0 && normal[1]==0 && normal[2]==0 ) continue;
if( depth<maxDepth ) coefficients[i] += normal;
if( depth )
{
const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.neighbors[depth-1];
for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
if( neighbors5.neighbors[x][y][z] )
{
TreeOctNode* _node = neighbors5.neighbors[x][y][z];
if( isInterior2 )
{
Point3D< double >& div = _stencil.values[x][y][z];
_constraints[t][ _node->nodeData.nodeIndex ] += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] );
}
else _constraints[t][ _node->nodeData.nodeIndex ] += GetDivergence( node , _node , normal );
}
}
}
}
}
#pragma omp parallel for num_threads( threads ) schedule( static )
for( int i=0 ; i<_sNodes.nodeCount[maxDepth] ; i++ )
{
Real cSum = Real(0.);
for( int t=0 ; t<threads ; t++ ) cSum += _constraints[t][i];
constraints[i] = cSum;
}
// Fine-to-coarse down-sampling of constraints
for( int d=maxDepth-1 ; d>=0 ; d-- ) DownSample( d , _sNodes , &constraints[0] );
// Coarse-to-fine up-sampling of coefficients
for( int d=0 ; d<maxDepth ; d++ ) UpSample( d , _sNodes , &coefficients[0] );
// Add the accumulated constraints from all finer depths
#pragma omp parallel for num_threads( threads )
for( int i=0 ; i<_sNodes.nodeCount[maxDepth] ; i++ ) _sNodes.treeNodes[i]->nodeData.constraint += constraints[i];
// Compute the contribution from all coarser depths
for( int d=0 ; d<=maxDepth ; d++ )
{
int start = _sNodes.nodeCount[d] , end = _sNodes.nodeCount[d+1] , range = end - start;
Stencil< Point3D< double > , 5 > stencils[2][2][2];
SetDivergenceStencils( d , stencils , false );
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
{
TreeOctNode::NeighborKey5 neighborKey5;
neighborKey5.set( maxDepth );
for( int i=start+(range*t)/threads ; i<start+(range*(t+1))/threads ; i++ )
{
TreeOctNode* node = _sNodes.treeNodes[i];
int depth = node->depth();
if( !depth ) continue;
int startX=0 , endX=5 , startY=0 , endY=5 , startZ=0 , endZ=5;
UpdateCoarserSupportBounds( node , startX , endX , startY , endY , startZ , endZ );
const TreeOctNode::Neighbors5& neighbors5 = neighborKey5.getNeighbors( node->parent );
bool isInterior;
{
int d , off[3];
node->depthAndOffset( d , off );
int mn = 4 , mx = (1<<d)-4;
isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
}
int cx , cy , cz;
if( d )
{
int c = int( node - node->parent->children );
Cube::FactorCornerIndex( c , cx , cy , cz );
}
else cx = cy = cz = 0;
Stencil< Point3D< double > , 5 >& _stencil = stencils[cx][cy][cz];
Real constraint = Real(0);
for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
if( neighbors5.neighbors[x][y][z] )
{
TreeOctNode* _node = neighbors5.neighbors[x][y][z];
int _i = _node->nodeData.nodeIndex;
if( isInterior )
{
Point3D< double >& div = _stencil.values[x][y][z];
Point3D< Real >& normal = coefficients[_i];
constraint += Real( div[0] * normal[0] + div[1] * normal[1] + div[2] * normal[2] );
}
else constraint += GetDivergence( _node , node , coefficients[_i] );
}
node->nodeData.constraint += constraint;
}
}
}
fData.clearDotTables( fData.DV_DOT_FLAG );
// Set the point weights for evaluating the iso-value
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ )
for( int i=(_sNodes.nodeCount[maxDepth+1]*t)/threads ; i<(_sNodes.nodeCount[maxDepth+1]*(t+1))/threads ; i++ )
{
TreeOctNode* temp = _sNodes.treeNodes[i];
if( temp->nodeData.nodeIndex<0 || temp->nodeData.normalIndex<0 ) temp->nodeData.centerWeightContribution = 0;
else temp->nodeData.centerWeightContribution = Real( Length((*normals)[temp->nodeData.normalIndex]) );
}
MemoryUsage();
delete normals;
normals = NULL;
}
template<int Degree>
void Octree<Degree>::AdjacencyCountFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencyCount++;}
template<int Degree>
void Octree<Degree>::AdjacencySetFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){adjacencies[adjacencyCount++]=node1->nodeData.nodeIndex;}
template<int Degree>
void Octree<Degree>::RefineFunction::Function( TreeOctNode* node1 , const TreeOctNode* node2 )
{
if( !node1->children && node1->depth()<depth ) node1->initChildren();
}
template< int Degree >
void Octree< Degree >::FaceEdgesFunction::Function( const TreeOctNode* node1 , const TreeOctNode* node2 )
{
if( !node1->children && MarchingCubes::HasRoots( node1->nodeData.mcIndex ) )
{
RootInfo ri1 , ri2;
int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES];
int count=MarchingCubes::AddTriangleIndices( node1->nodeData.mcIndex , isoTri );
for( int j=0 ; j<count ; j++ )
for( int k=0 ; k<3 ; k++ )
if( fIndex==Cube::FaceAdjacentToEdges( isoTri[j*3+k] , isoTri[j*3+((k+1)%3)] ) )
if( GetRootIndex( node1 , isoTri[j*3+k] , maxDepth , ri1 ) && GetRootIndex( node1 , isoTri[j*3+((k+1)%3)] , maxDepth , ri2 ) )
{
long long key1=ri1.key , key2=ri2.key;
edges->push_back( std::pair< RootInfo , RootInfo >( ri2 , ri1 ) );
if( vertexCount->count( key1 )==0 )
{
(*vertexCount)[key1].first = ri1;
(*vertexCount)[key1].second=0;
}
if( vertexCount->count( key2 )==0 )
{
(*vertexCount)[key2].first = ri2;
(*vertexCount)[key2].second=0;
}
(*vertexCount)[key1].second--;
(*vertexCount)[key2].second++;
}
else fprintf( stderr , "Bad Edge 1: %lld %lld\n" , ri1.key , ri2.key );
}
}
template< int Degree >
void Octree< Degree >::RefineBoundary( int subdivideDepth )
{
// This implementation is somewhat tricky.
// We would like to ensure that leaf-nodes across a subdivision boundary have the same depth.
// We do this by calling the setNeighbors function.
// The key is to implement this in a single pass through the leaves, ensuring that refinements don't propogate.
// To this end, we do the minimal refinement that ensures that a cross boundary neighbor, and any of its cross-boundary
// neighbors are all refined simultaneously.
// For this reason, the implementation can only support nodes deeper than sDepth.
bool flags[3][3][3];
int maxDepth = tree.maxDepth();
int sDepth;
if( subdivideDepth<=0 ) sDepth = 0;
else sDepth = maxDepth-subdivideDepth;
if( sDepth<=0 ) return;
// Ensure that face adjacent neighbors across the subdivision boundary exist to allow for
// a consistent definition of the iso-surface
TreeOctNode::NeighborKey3 nKey;
nKey.set( maxDepth );
for( TreeOctNode* leaf=tree.nextLeaf() ; leaf ; leaf=tree.nextLeaf( leaf ) )
if( leaf->depth()>sDepth )
{
int d , off[3] , _off[3];
leaf->depthAndOffset( d , off );
int res = (1<<d)-1 , _res = ( 1<<(d-sDepth) )-1;
_off[0] = off[0]&_res , _off[1] = off[1]&_res , _off[2] = off[2]&_res;
bool boundary[3][2] =
{
{ (off[0]!=0 && _off[0]==0) , (off[0]!=res && _off[0]==_res) } ,
{ (off[1]!=0 && _off[1]==0) , (off[1]!=res && _off[1]==_res) } ,
{ (off[2]!=0 && _off[2]==0) , (off[2]!=res && _off[2]==_res) }
};
if( boundary[0][0] || boundary[0][1] || boundary[1][0] || boundary[1][1] || boundary[2][0] || boundary[2][1] )
{
TreeOctNode::Neighbors3& neighbors = nKey.getNeighbors( leaf );
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;
int x=0 , y=0 , z=0;
if ( boundary[0][0] && !neighbors.neighbors[0][1][1] ) x = -1;
else if( boundary[0][1] && !neighbors.neighbors[2][1][1] ) x = 1;
if ( boundary[1][0] && !neighbors.neighbors[1][0][1] ) y = -1;
else if( boundary[1][1] && !neighbors.neighbors[1][2][1] ) y = 1;
if ( boundary[2][0] && !neighbors.neighbors[1][1][0] ) z = -1;
else if( boundary[2][1] && !neighbors.neighbors[1][1][2] ) z = 1;
if( x || y || z )
{
// Corner case
if( x && y && z ) flags[1+x][1+y][1+z] = true;
// Edge cases
if( x && y ) flags[1+x][1+y][1 ] = true;
if( x && z ) flags[1+x][1 ][1+z] = true;
if( y && z ) flags[1 ][1+y][1+1] = true;
// Face cases
if( x ) flags[1+x][1 ][1 ] = true;
if( y ) flags[1 ][1+y][1 ] = true;
if( z ) flags[1 ][1 ][1+z] = true;
nKey.setNeighbors( leaf , flags );
}
}
}
_sNodes.set( tree );
MemoryUsage();
}
template<int Degree>
void Octree<Degree>::GetMCIsoTriangles( Real isoValue , int subdivideDepth , CoredMeshData* mesh , int fullDepthIso , int nonLinearFit , bool addBarycenter , bool polygonMesh )
{
fData.setValueTables( fData.VALUE_FLAG | fData.D_VALUE_FLAG , 0 , postNormalSmooth );
// Ensure that the subtrees are self-contained
RefineBoundary( subdivideDepth );
RootData rootData , coarseRootData;
std::vector< Point3D< float > >* interiorPoints;
int maxDepth = tree.maxDepth();
int sDepth = subdivideDepth<=0 ? 0 : std::max< int >( 0 , maxDepth-subdivideDepth );
std::vector< Real > metSolution( _sNodes.nodeCount[maxDepth] , 0 );
#pragma omp parallel for num_threads( threads )
for( int i=_sNodes.nodeCount[_minDepth] ; i<_sNodes.nodeCount[maxDepth] ; i++ ) metSolution[i] = _sNodes.treeNodes[i]->nodeData.solution;
for( int d=0 ; d<maxDepth ; d++ ) UpSample( d , _sNodes , &metSolution[0] );
// Clear the marching cube indices
#pragma omp parallel for num_threads( threads )
for( int i=0 ; i<_sNodes.nodeCount[maxDepth+1] ; i++ ) _sNodes.treeNodes[i]->nodeData.mcIndex = 0;
rootData.boundaryValues = new std::unordered_map< long long , std::pair< Real , Point3D< Real > > >();
int offSet = 0;
int maxCCount = _sNodes.getMaxCornerCount( &tree , sDepth , maxDepth , threads );
int maxECount = _sNodes.getMaxEdgeCount ( &tree , sDepth , threads );
rootData.cornerValues = new Real [ maxCCount ];
rootData.cornerNormals = new Point3D< Real >[ maxCCount ];
rootData.interiorRoots = new int [ maxECount ];
rootData.cornerValuesSet = new char[ maxCCount ];
rootData.cornerNormalsSet = new char[ maxCCount ];
rootData.edgesSet = new char[ maxECount ];
_sNodes.setCornerTable( coarseRootData , &tree , sDepth , threads );
coarseRootData.cornerValues = new Real[ coarseRootData.cCount ];
coarseRootData.cornerNormals = new Point3D< Real >[ coarseRootData.cCount ];
coarseRootData.cornerValuesSet = new char[ coarseRootData.cCount ];
coarseRootData.cornerNormalsSet = new char[ coarseRootData.cCount ];
memset( coarseRootData.cornerValuesSet , 0 , sizeof( char ) * coarseRootData.cCount );
memset( coarseRootData.cornerNormalsSet , 0 , sizeof( char ) * coarseRootData.cCount );
MemoryUsage();
std::vector< TreeOctNode::ConstNeighborKey3 > nKeys( threads );
for( int t=0 ; t<threads ; t++ ) nKeys[t].set( maxDepth );
TreeOctNode::ConstNeighborKey3 nKey;
std::vector< TreeOctNode::ConstNeighborKey5 > nKeys5( threads );
for( int t=0 ; t<threads ; t++ ) nKeys5[t].set( maxDepth );
TreeOctNode::ConstNeighborKey5 nKey5;
nKey5.set( maxDepth );
nKey.set( maxDepth );
// First process all leaf nodes at depths strictly finer than sDepth, one subtree at a time.
for( int i=_sNodes.nodeCount[sDepth] ; i<_sNodes.nodeCount[sDepth+1] ; i++ )
{
if( !_sNodes.treeNodes[i]->children ) continue;
_sNodes.setCornerTable( rootData , _sNodes.treeNodes[i] , threads );
_sNodes.setEdgeTable ( rootData , _sNodes.treeNodes[i] , threads );
memset( rootData.cornerValuesSet , 0 , sizeof( char ) * rootData.cCount );
memset( rootData.cornerNormalsSet , 0 , sizeof( char ) * rootData.cCount );
memset( rootData.edgesSet , 0 , sizeof( char ) * rootData.eCount );
interiorPoints = new std::vector< Point3D< float > >();
for( int d=maxDepth ; d>sDepth ; d-- )
{
int leafNodeCount = 0;
std::vector< TreeOctNode* > leafNodes;
for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodeCount++;
leafNodes.reserve( leafNodeCount );
for( TreeOctNode* node=_sNodes.treeNodes[i]->nextLeaf() ; node ; node=_sNodes.treeNodes[i]->nextLeaf( node ) ) if( node->d==d ) leafNodes.push_back( node );
Stencil< double , 3 > stencil1[8] , stencil2[8][8];
SetEvaluationStencils( d , stencil1 , stencil2 );
// First set the corner values and associated marching-cube indices
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ ) for( int i=(leafNodeCount*t)/threads ; i<(leafNodeCount*(t+1))/threads ; i++ )
{
TreeOctNode* leaf = leafNodes[i];
SetIsoCorners( isoValue , leaf , rootData , rootData.cornerValuesSet , rootData.cornerValues , nKeys[t] , &metSolution[0] , stencil1 , stencil2 );
// If this node shares a vertex with a coarser node, set the vertex value
int d , off[3];
leaf->depthAndOffset( d , off );
int res = 1<<(d-sDepth);
off[0] %= res , off[1] %=res , off[2] %= res;
res--;
if( !(off[0]%res) && !(off[1]%res) && !(off[2]%res) )
{
const TreeOctNode* temp = leaf;
while( temp->d!=sDepth ) temp = temp->parent;
int x = off[0]==0 ? 0 : 1 , y = off[1]==0 ? 0 : 1 , z = off[2]==0 ? 0 : 1;
int c = Cube::CornerIndex( x , y , z );
int idx = coarseRootData.cornerIndices( temp )[ c ];
coarseRootData.cornerValues[ idx ] = rootData.cornerValues[ rootData.cornerIndices( leaf )[c] ];
coarseRootData.cornerValuesSet[ idx ] = true;
}
// Compute the iso-vertices
if( MarchingCubes::HasRoots( leaf->nodeData.mcIndex ) )
SetMCRootPositions( leaf , sDepth , isoValue , nKeys5[t] , rootData , interiorPoints , mesh , &metSolution[0] , nonLinearFit );
}
// Note that this should be broken off for multi-threading as
// the SetMCRootPositions writes to interiorPoints (with lockupdateing)
// while GetMCIsoTriangles reads from interiorPoints (without locking)
#if MISHA_DEBUG
std::vector< Point3D< float > > barycenters;
std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? & barycenters : NULL;
#endif // MISHA_DEBUG
#pragma omp parallel for num_threads( threads )
for( int t=0 ; t<threads ; t++ ) for( int i=(leafNodeCount*t)/threads ; i<(leafNodeCount*(t+1))/threads ; i++ )
{
TreeOctNode* leaf = leafNodes[i];
if( MarchingCubes::HasRoots( leaf->nodeData.mcIndex ) )
#if MISHA_DEBUG
GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , polygonMesh , barycenterPtr );
#else // !MISHA_DEBUG
GetMCIsoTriangles( leaf , mesh , rootData , interiorPoints , offSet , sDepth , addBarycenter , polygonMesh );
#endif // MISHA_DEBUG
}
#if MISHA_DEBUG
for( int i=0 ; i<barycenters.size() ; i++ ) interiorPoints->push_back( barycenters[i] );
#endif // MISHA_DEBUG
}
offSet = mesh->outOfCorePointCount();
#if 1
delete interiorPoints;
#endif
}
MemoryUsage();
delete[] rootData.cornerValues , delete[] rootData.cornerNormals , rootData.cornerValues = NULL , rootData.cornerNormals = NULL;
delete[] rootData.cornerValuesSet , delete[] rootData.cornerNormalsSet , rootData.cornerValuesSet = NULL , rootData.cornerNormalsSet = NULL;
delete[] rootData.interiorRoots ; rootData.interiorRoots = NULL;
delete[] rootData.edgesSet ; rootData.edgesSet = NULL;
coarseRootData.interiorRoots = NULL;
coarseRootData.boundaryValues = rootData.boundaryValues;
for( auto iter=rootData.boundaryRoots.cbegin() ; iter!=rootData.boundaryRoots.cend() ; iter++ )
coarseRootData.boundaryRoots[iter->first] = iter->second;
for( int d=sDepth ; d>=0 ; d-- )
{
Stencil< double , 3 > stencil1[8] , stencil2[8][8];
SetEvaluationStencils( d , stencil1 , stencil2 );
#if MISHA_DEBUG
std::vector< Point3D< float > > barycenters;
std::vector< Point3D< float > >* barycenterPtr = addBarycenter ? &barycenters : NULL;
#endif // MISHA_DEBUG
for( int i=_sNodes.nodeCount[d] ; i<_sNodes.nodeCount[d+1] ; i++ )
{
TreeOctNode* leaf = _sNodes.treeNodes[i];
if( leaf->children ) continue;
// First set the corner values and associated marching-cube indices
SetIsoCorners( isoValue , leaf , coarseRootData , coarseRootData.cornerValuesSet , coarseRootData.cornerValues , nKey , &metSolution[0] , stencil1 , stencil2 );
// Now compute the iso-vertices
if( MarchingCubes::HasRoots( leaf->nodeData.mcIndex ) )
{
SetMCRootPositions( leaf , 0 , isoValue , nKey5 , coarseRootData , NULL , mesh , &metSolution[0] , nonLinearFit );
#if MISHA_DEBUG
GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , polygonMesh , barycenterPtr );
#else // !MISHA_DEBUG
GetMCIsoTriangles( leaf , mesh , coarseRootData , NULL , 0 , 0 , addBarycenter , polygonMesh );
#endif // MISHA_DEBUG
}
}
}
MemoryUsage();
delete[] coarseRootData.cornerValues , delete[] coarseRootData.cornerNormals;
delete[] coarseRootData.cornerValuesSet , delete[] coarseRootData.cornerNormalsSet;
delete rootData.boundaryValues;
}
template<int Degree>
Real Octree<Degree>::getCenterValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey , const TreeOctNode* node){
int idx[3];
Real value=0;
VertexData::CenterIndex(node,fData.depth,idx);
idx[0]*=fData.functionCount;
idx[1]*=fData.functionCount;
idx[2]*=fData.functionCount;
int minDepth = std::max< int >( 0 , std::min< int >( _minDepth , node->depth()-1 ) );
for( int i=minDepth ; i<=node->depth() ; i++ )
for(int j=0;j<3;j++)
for(int k=0;k<3;k++)
for(int l=0;l<3;l++)
{
const TreeOctNode* n=neighborKey.neighbors[i].neighbors[j][k][l];
if( n )
{
Real temp=n->nodeData.solution;
value+=temp*Real(
fData.valueTables[idx[0]+int(n->off[0])]*
fData.valueTables[idx[1]+int(n->off[1])]*
fData.valueTables[idx[2]+int(n->off[2])]);
}
}
if(node->children)
{
for(int i=0;i<Cube::CORNERS;i++){
int ii=Cube::AntipodalCornerIndex(i);
const TreeOctNode* n=&node->children[i];
while(1){
value+=n->nodeData.solution*Real(
fData.valueTables[idx[0]+int(n->off[0])]*
fData.valueTables[idx[1]+int(n->off[1])]*
fData.valueTables[idx[2]+int(n->off[2])]);
if( n->children ) n=&n->children[ii];
else break;
}
}
}
return value;
}
template< int Degree >
Real Octree< Degree >::getCornerValue( const OctNode< TreeNodeData , Real >::ConstNeighborKey3& neighborKey3 , const TreeOctNode* node , int corner , const Real* metSolution )
{
int idx[3];
double value = 0;
VertexData::CornerIndex( node , corner , fData.depth , idx );
idx[0] *= fData.functionCount;
idx[1] *= fData.functionCount;
idx[2] *= fData.functionCount;
int d = node->depth();
int cx , cy , cz;
int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3;
Cube::FactorCornerIndex( corner , cx , cy , cz );
{
TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d];
if( cx==0 ) endX = 2;
else startX = 1;
if( cy==0 ) endY = 2;
else startY = 1;
if( cz==0 ) endZ = 2;
else startZ = 1;
for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
{
const TreeOctNode* n=neighbors.neighbors[x][y][z];
if( n )
{
double v =
fData.valueTables[ idx[0]+int(n->off[0]) ]*
fData.valueTables[ idx[1]+int(n->off[1]) ]*
fData.valueTables[ idx[2]+int(n->off[2]) ];
value += n->nodeData.solution * v;
}
}
}
if( d>0 && d>_minDepth )
{
int _corner = int( node - node->parent->children );
int _cx , _cy , _cz;
Cube::FactorCornerIndex( _corner , _cx , _cy , _cz );
if( cx!=_cx ) startX = 0 , endX = 3;
if( cy!=_cy ) startY = 0 , endY = 3;
if( cz!=_cz ) startZ = 0 , endZ = 3;
TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1];
for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
{
const TreeOctNode* n=neighbors.neighbors[x][y][z];
if( n )
{
double v =
fData.valueTables[ idx[0]+int(n->off[0]) ]*
fData.valueTables[ idx[1]+int(n->off[1]) ]*
fData.valueTables[ idx[2]+int(n->off[2]) ];
value += metSolution[ n->nodeData.nodeIndex ] * v;
}
}
}
return Real( value );
}
template< int Degree >
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] )
{
double value = 0;
int d = node->depth();
int cx , cy , cz;
int startX = 0 , endX = 3 , startY = 0 , endY = 3 , startZ = 0 , endZ = 3;
Cube::FactorCornerIndex( corner , cx , cy , cz );
{
TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d];
if( cx==0 ) endX = 2;
else startX = 1;
if( cy==0 ) endY = 2;
else startY = 1;
if( cz==0 ) endZ = 2;
else startZ = 1;
for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
{
const TreeOctNode* n=neighbors.neighbors[x][y][z];
if( n ) value += n->nodeData.solution * stencil1[x][y][z];
}
}
if( d>0 && d>_minDepth )
{
int _corner = int( node - node->parent->children );
int _cx , _cy , _cz;
Cube::FactorCornerIndex( _corner , _cx , _cy , _cz );
if( cx!=_cx ) startX = 0 , endX = 3;
if( cy!=_cy ) startY = 0 , endY = 3;
if( cz!=_cz ) startZ = 0 , endZ = 3;
TreeOctNode::ConstNeighbors3& neighbors = neighborKey3.neighbors[d-1];
for( int x=startX ; x<endX ; x++ ) for( int y=startY ; y<endY ; y++ ) for( int z=startZ ; z<endZ ; z++ )
{
const TreeOctNode* n=neighbors.neighbors[x][y][z];
if( n ) value += metSolution[ n->nodeData.nodeIndex ] * stencil2[x][y][z];
}
}
return Real( value );
}
template< int Degree >
Point3D< Real > Octree< Degree >::getCornerNormal( const OctNode< TreeNodeData , Real >::ConstNeighborKey5& neighborKey5 , const TreeOctNode* node , int corner , const Real* metSolution )
{
int idx[3];
Point3D< Real > normal;
normal[0] = normal[1] = normal[2] = 0.;
VertexData::CornerIndex( node , corner , fData.depth , idx );
idx[0] *= fData.functionCount;
idx[1] *= fData.functionCount;
idx[2] *= fData.functionCount;
int d = node->depth();
// Iterate over all ancestors that can overlap the corner
{
TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d];
for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ )
{
const TreeOctNode* n=neighbors.neighbors[j][k][l];
if( n )
{
int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] };
double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] };
double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] };
Real solution = n->nodeData.solution;
normal[0] += Real( dValues[0] * values[1] * values[2] * solution );
normal[1] += Real( values[0] * dValues[1] * values[2] * solution );
normal[2] += Real( values[0] * values[1] * dValues[2] * solution );
}
}
}
if( d>0 && d>_minDepth )
{
TreeOctNode::ConstNeighbors5& neighbors = neighborKey5.neighbors[d-1];
for( int j=0 ; j<5 ; j++ ) for( int k=0 ; k<5 ; k++ ) for( int l=0 ; l<5 ; l++ )
{
const TreeOctNode* n=neighbors.neighbors[j][k][l];
if( n )
{
int _idx[] = { idx[0] + n->off[0] , idx[1] + n->off[1] , idx[2] + n->off[2] };
double values[] = { fData.valueTables[_idx[0]] , fData.valueTables[_idx[1]] , fData.valueTables[_idx[2]] };
double dValues[] = { fData.dValueTables[_idx[0]] , fData.dValueTables[_idx[1]] , fData.dValueTables[_idx[2]] };
Real solution = metSolution[ n->nodeData.nodeIndex ];
normal[0] += Real( dValues[0] * values[1] * values[2] * solution );
normal[1] += Real( values[0] * dValues[1] * values[2] * solution );
normal[2] += Real( values[0] * values[1] * dValues[2] * solution );
}
}
}
return normal;
}
template< int Degree >
Real Octree<Degree>::GetIsoValue( void )
{
Real isoValue , weightSum;
neighborKey2.set( fData.depth );
fData.setValueTables( fData.VALUE_FLAG , 0 );
isoValue = weightSum = 0;
#pragma omp parallel for num_threads( threads ) reduction( + : isoValue , weightSum )
for( int t=0 ; t<threads ; t++)
{
TreeOctNode::ConstNeighborKey3 nKey;
nKey.set( _sNodes.maxDepth-1 );
int nodeCount = _sNodes.nodeCount[ _sNodes.maxDepth ];
for( int i=(nodeCount*t)/threads ; i<(nodeCount*(t+1))/threads ; i++ )
{
TreeOctNode* temp = _sNodes.treeNodes[i];
nKey.getNeighbors( temp );
Real w = temp->nodeData.centerWeightContribution;
if( w!=0 )
{
isoValue += getCenterValue( nKey , temp ) * w;
weightSum += w;
}
}
}
return isoValue/weightSum;
}
template< int Degree >
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] )
{
Real cornerValues[ Cube::CORNERS ];
const SortedTreeNodes::CornerIndices& cIndices = cData[ leaf ];
bool isInterior;
int d , off[3];
leaf->depthAndOffset( d , off );
int mn = 2 , mx = (1<<d)-2;
isInterior = ( off[0]>=mn && off[0]<mx && off[1]>=mn && off[1]<mx && off[2]>=mn && off[2]<mx );
nKey.getNeighbors( leaf );
for( int c=0 ; c<Cube::CORNERS ; c++ )
{
int vIndex = cIndices[c];
if( valuesSet[vIndex] ) cornerValues[c] = values[vIndex];
else
{
if( isInterior ) cornerValues[c] = getCornerValue( nKey , leaf , c , metSolution , stencil1[c].values , stencil2[int(leaf - leaf->parent->children)][c].values );
else cornerValues[c] = getCornerValue( nKey , leaf , c , metSolution );
values[vIndex] = cornerValues[c];
valuesSet[vIndex] = 1;
}
}
leaf->nodeData.mcIndex = MarchingCubes::GetIndex( cornerValues , isoValue );
// Set the marching cube indices for all interior nodes.
if( leaf->parent )
{
TreeOctNode* parent = leaf->parent;
int c = int( leaf - leaf->parent->children );
int mcid = leaf->nodeData.mcIndex & (1<<MarchingCubes::cornerMap()[c]);
if( mcid )
{
poisson::atomicOr(parent->nodeData.mcIndex, mcid);
while( 1 )
{
if( parent->parent && parent->parent->d>=_minDepth && (parent-parent->parent->children)==c )
{
poisson::atomicOr(parent->parent->nodeData.mcIndex, mcid);
parent = parent->parent;
}
else break;
}
}
}
}
template<int Degree>
int Octree<Degree>::InteriorFaceRootCount(const TreeOctNode* node,const int &faceIndex,int maxDepth){
int c1,c2,e1,e2,dir,off,cnt=0;
int corners[Cube::CORNERS/2];
if(node->children){
Cube::FaceCorners(faceIndex,corners[0],corners[1],corners[2],corners[3]);
Cube::FactorFaceIndex(faceIndex,dir,off);
c1=corners[0];
c2=corners[3];
switch(dir){
case 0:
e1=Cube::EdgeIndex(1,off,1);
e2=Cube::EdgeIndex(2,off,1);
break;
case 1:
e1=Cube::EdgeIndex(0,off,1);
e2=Cube::EdgeIndex(2,1,off);
break;
case 2:
e1=Cube::EdgeIndex(0,1,off);
e2=Cube::EdgeIndex(1,1,off);
break;
};
cnt+=EdgeRootCount(&node->children[c1],e1,maxDepth)+EdgeRootCount(&node->children[c1],e2,maxDepth);
switch(dir){
case 0:
e1=Cube::EdgeIndex(1,off,0);
e2=Cube::EdgeIndex(2,off,0);
break;
case 1:
e1=Cube::EdgeIndex(0,off,0);
e2=Cube::EdgeIndex(2,0,off);
break;
case 2:
e1=Cube::EdgeIndex(0,0,off);
e2=Cube::EdgeIndex(1,0,off);
break;
};
cnt+=EdgeRootCount(&node->children[c2],e1,maxDepth)+EdgeRootCount(&node->children[c2],e2,maxDepth);
for(int i=0;i<Cube::CORNERS/2;i++){if(node->children[corners[i]].children){cnt+=InteriorFaceRootCount(&node->children[corners[i]],faceIndex,maxDepth);}}
}
return cnt;
}
template<int Degree>
int Octree<Degree>::EdgeRootCount(const TreeOctNode* node,int edgeIndex,int maxDepth){
int f1,f2,c1,c2;
const TreeOctNode* temp;
Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);
int eIndex;
const TreeOctNode* finest=node;
eIndex=edgeIndex;
if(node->depth()<maxDepth){
temp=node->faceNeighbor(f1);
if(temp && temp->children){
finest=temp;
eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1);
}
else{
temp=node->faceNeighbor(f2);
if(temp && temp->children){
finest=temp;
eIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2);
}
else{
temp=node->edgeNeighbor(edgeIndex);
if(temp && temp->children){
finest=temp;
eIndex=Cube::EdgeReflectEdgeIndex(edgeIndex);
}
}
}
}
Cube::EdgeCorners(eIndex,c1,c2);
if(finest->children) return EdgeRootCount(&finest->children[c1],eIndex,maxDepth)+EdgeRootCount(&finest->children[c2],eIndex,maxDepth);
else return MarchingCubes::HasEdgeRoots(finest->nodeData.mcIndex,eIndex);
}
template<int Degree>
int Octree<Degree>::IsBoundaryFace(const TreeOctNode* node,int faceIndex,int subdivideDepth){
int dir,offset,d,o[3],idx;
if(subdivideDepth<0){return 0;}
if(node->d<=subdivideDepth){return 1;}
Cube::FactorFaceIndex(faceIndex,dir,offset);
node->depthAndOffset(d,o);
idx=(int(o[dir])<<1) + (offset<<1);
return !(idx%(2<<(int(node->d)-subdivideDepth)));
}
template<int Degree>
int Octree<Degree>::IsBoundaryEdge(const TreeOctNode* node,int edgeIndex,int subdivideDepth){
int dir,x,y;
Cube::FactorEdgeIndex(edgeIndex,dir,x,y);
return IsBoundaryEdge(node,dir,x,y,subdivideDepth);
}
template<int Degree>
int Octree<Degree>::IsBoundaryEdge( const TreeOctNode* node , int dir , int x , int y , int subdivideDepth )
{
int d , o[3] , idx1 , idx2 , mask;
if( subdivideDepth<0 ) return 0;
if( node->d<=subdivideDepth ) return 1;
node->depthAndOffset( d , o );
switch( dir )
{
case 0:
idx1 = o[1] + x;
idx2 = o[2] + y;
break;
case 1:
idx1 = o[0] + x;
idx2 = o[2] + y;
break;
case 2:
idx1 = o[0] + x;
idx2 = o[1] + y;
break;
}
mask = 1<<( int(node->d) - subdivideDepth );
return !(idx1%(mask)) || !(idx2%(mask));
}
template< int Degree >
void Octree< Degree >::GetRootSpan( const RootInfo& ri , Point3D< float >& start , Point3D< float >& end )
{
int o , i1 , i2;
Real width;
Point3D< Real > c;
Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 );
ri.node->centerAndWidth( c , width );
switch(o)
{
case 0:
start[0] = c[0] - width/2;
end [0] = c[0] + width/2;
start[1] = end[1] = c[1] - width/2 + width*i1;
start[2] = end[2] = c[2] - width/2 + width*i2;
break;
case 1:
start[0] = end[0] = c[0] - width/2 + width*i1;
start[1] = c[1] - width/2;
end [1] = c[1] + width/2;
start[2] = end[2] = c[2] - width/2 + width*i2;
break;
case 2:
start[0] = end[0] = c[0] - width/2 + width*i1;
start[1] = end[1] = c[1] - width/2 + width*i2;
start[2] = c[2] - width/2;
end [2] = c[2] + width/2;
break;
}
}
//////////////////////////////////////////////////////////////////////////////////////
// The assumption made when calling this code is that the edge has at most one root //
//////////////////////////////////////////////////////////////////////////////////////
template< int Degree >
int Octree< Degree >::GetRoot( const RootInfo& ri , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , Point3D< Real > & position , RootData& rootData , int sDepth , const Real* metSolution , int nonLinearFit )
{
if( !MarchingCubes::HasRoots( ri.node->nodeData.mcIndex ) ) return 0;
int c1 , c2;
Cube::EdgeCorners( ri.edgeIndex , c1 , c2 );
if( !MarchingCubes::HasEdgeRoots( ri.node->nodeData.mcIndex , ri.edgeIndex ) ) return 0;
long long key1 , key2;
Point3D< Real > n[2];
int i , o , i1 , i2 , rCount=0;
Polynomial<2> P;
std::vector< double > roots;
double x0 , x1;
Real center , width;
Real averageRoot=0;
Cube::FactorEdgeIndex( ri.edgeIndex , o , i1 , i2 );
int idx1[3] , idx2[3];
key1 = VertexData::CornerIndex( ri.node , c1 , fData.depth , idx1 );
key2 = VertexData::CornerIndex( ri.node , c2 , fData.depth , idx2 );
bool isBoundary = ( IsBoundaryEdge( ri.node , ri.edgeIndex , sDepth )!=0 );
bool haveKey1 , haveKey2;
std::pair< Real , Point3D< Real > > keyValue1 , keyValue2;
int iter1 , iter2;
{
iter1 = rootData.cornerIndices( ri.node )[ c1 ];
iter2 = rootData.cornerIndices( ri.node )[ c2 ];
keyValue1.first = rootData.cornerValues[iter1];
keyValue2.first = rootData.cornerValues[iter2];
if( isBoundary )
{
#pragma omp critical (normal_hash_access)
{
haveKey1 = ( rootData.boundaryValues->count( key1 )>0 );
haveKey2 = ( rootData.boundaryValues->count( key2 )>0 );
if( haveKey1 ) keyValue1 = (*rootData.boundaryValues)[key1];
if( haveKey2 ) keyValue2 = (*rootData.boundaryValues)[key2];
}
}
else
{
haveKey1 = ( rootData.cornerNormalsSet[ iter1 ] != 0 );
haveKey2 = ( rootData.cornerNormalsSet[ iter2 ] != 0 );
keyValue1.first = rootData.cornerValues[iter1];
keyValue2.first = rootData.cornerValues[iter2];
if( haveKey1 ) keyValue1.second = rootData.cornerNormals[iter1];
if( haveKey2 ) keyValue2.second = rootData.cornerNormals[iter2];
}
}
if( !haveKey1 || !haveKey2 ) neighborKey5.getNeighbors( ri.node );
if( !haveKey1 ) keyValue1.second = getCornerNormal( neighborKey5 , ri.node , c1 , metSolution );
x0 = keyValue1.first;
n[0] = keyValue1.second;
if( !haveKey2 ) keyValue2.second = getCornerNormal( neighborKey5 , ri.node , c2 , metSolution );
x1 = keyValue2.first;
n[1] = keyValue2.second;
if( !haveKey1 || !haveKey2 )
{
if( isBoundary )
{
#pragma omp critical (normal_hash_access)
{
if( !haveKey1 ) (*rootData.boundaryValues)[key1] = keyValue1;
if( !haveKey2 ) (*rootData.boundaryValues)[key2] = keyValue2;
}
}
else
{
if( !haveKey1 ) rootData.cornerNormals[iter1] = keyValue1.second , rootData.cornerNormalsSet[ iter1 ] = 1;
if( !haveKey2 ) rootData.cornerNormals[iter2] = keyValue2.second , rootData.cornerNormalsSet[ iter2 ] = 1;
}
}
Point3D< Real > c;
ri.node->centerAndWidth(c,width);
center=c[o];
for( i=0 ; i<DIMENSION ; i++ ) n[0][i] *= width , n[1][i] *= width;
switch(o)
{
case 0:
position[1] = c[1]-width/2+width*i1;
position[2] = c[2]-width/2+width*i2;
break;
case 1:
position[0] = c[0]-width/2+width*i1;
position[2] = c[2]-width/2+width*i2;
break;
case 2:
position[0] = c[0]-width/2+width*i1;
position[1] = c[1]-width/2+width*i2;
break;
}
double dx0,dx1;
dx0 = n[0][o];
dx1 = n[1][o];
// The scaling will turn the Hermite Spline into a quadratic
double scl=(x1-x0)/((dx1+dx0)/2);
dx0 *= scl;
dx1 *= scl;
// Hermite Spline
P.coefficients[0] = x0;
P.coefficients[1] = dx0;
P.coefficients[2] = 3*(x1-x0)-dx1-2*dx0;
P.getSolutions( isoValue , roots , EPSILON );
for( i=0 ; i<int(roots.size()) ; i++ )
if( roots[i]>=0 && roots[i]<=1 )
{
averageRoot += Real( roots[i] );
rCount++;
}
if( rCount && nonLinearFit ) averageRoot /= rCount;
else averageRoot = Real((x0-isoValue)/(x0-x1));
if( averageRoot<0 || averageRoot>1 )
{
fprintf( stderr , "[WARNING] Bad average root: %f\n" , averageRoot );
fprintf( stderr , "\t(%f %f) , (%f %f) (%f)\n" , x0 , x1 , dx0 , dx1 , isoValue );
if( averageRoot<0 ) averageRoot = 0;
if( averageRoot>1 ) averageRoot = 1;
}
position[o] = Real(center-width/2+width*averageRoot);
return 1;
}
template< int Degree >
int Octree< Degree >::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , int sDepth,RootInfo& ri )
{
int c1,c2,f1,f2;
const TreeOctNode *temp,*finest;
int finestIndex;
Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);
finest=node;
finestIndex=edgeIndex;
if(node->depth()<maxDepth){
if(IsBoundaryFace(node,f1,sDepth)){temp=NULL;}
else{temp=node->faceNeighbor(f1);}
if(temp && temp->children){
finest=temp;
finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f1);
}
else{
if(IsBoundaryFace(node,f2,sDepth)){temp=NULL;}
else{temp=node->faceNeighbor(f2);}
if(temp && temp->children){
finest=temp;
finestIndex=Cube::FaceReflectEdgeIndex(edgeIndex,f2);
}
else{
if(IsBoundaryEdge(node,edgeIndex,sDepth)){temp=NULL;}
else{temp=node->edgeNeighbor(edgeIndex);}
if(temp && temp->children){
finest=temp;
finestIndex=Cube::EdgeReflectEdgeIndex(edgeIndex);
}
}
}
}
Cube::EdgeCorners(finestIndex,c1,c2);
if(finest->children){
if (GetRootIndex(&finest->children[c1],finestIndex,maxDepth,sDepth,ri)) {return 1;}
else if (GetRootIndex(&finest->children[c2],finestIndex,maxDepth,sDepth,ri)) {return 1;}
else
{
fprintf( stderr , "[WARNING] Couldn't find root index with either child\n" );
return 0;
}
}
else
{
if( !(MarchingCubes::edgeMask()[finest->nodeData.mcIndex] & (1<<finestIndex)) )
{
fprintf( stderr , "[WARNING] Finest node does not have iso-edge\n" );
return 0;
}
int o,i1,i2;
Cube::FactorEdgeIndex(finestIndex,o,i1,i2);
int d,off[3];
finest->depthAndOffset(d,off);
ri.node=finest;
ri.edgeIndex=finestIndex;
int eIndex[2],offset;
offset=BinaryNode<Real>::Index( d , off[o] );
switch(o)
{
case 0:
eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
break;
case 1:
eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
break;
case 2:
eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
break;
}
ri.key = (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45;
return 1;
}
}
template<int Degree>
int Octree<Degree>::GetRootIndex( const TreeOctNode* node , int edgeIndex , int maxDepth , RootInfo& ri )
{
int c1,c2,f1,f2;
const TreeOctNode *temp,*finest;
int finestIndex;
// The assumption is that the super-edge has a root along it.
if(!(MarchingCubes::edgeMask()[node->nodeData.mcIndex] & (1<<edgeIndex))){return 0;}
Cube::FacesAdjacentToEdge(edgeIndex,f1,f2);
finest = node;
finestIndex = edgeIndex;
if( node->depth()<maxDepth && !node->children )
{
temp=node->faceNeighbor( f1 );
if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f1 );
else
{
temp = node->faceNeighbor( f2 );
if( temp && temp->children ) finest = temp , finestIndex = Cube::FaceReflectEdgeIndex( edgeIndex , f2 );
else
{
temp = node->edgeNeighbor( edgeIndex );
if( temp && temp->children ) finest = temp , finestIndex = Cube::EdgeReflectEdgeIndex( edgeIndex );
}
}
}
Cube::EdgeCorners( finestIndex , c1 , c2 );
if( finest->children )
{
if ( GetRootIndex( finest->children + c1 , finestIndex , maxDepth , ri ) ) return 1;
else if( GetRootIndex( finest->children + c2 , finestIndex , maxDepth , ri ) ) return 1;
else
{
int d1 , off1[3] , d2 , off2[3];
node->depthAndOffset( d1 , off1 );
finest->depthAndOffset( d2 , off2 );
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 );
printf( "\t" );
for( int i=0 ; i<8 ; i++ ) if( node->nodeData.mcIndex & (1<<i) ) printf( "1" ); else printf( "0" );
printf( "\t" );
for( int i=0 ; i<8 ; i++ ) if( finest->nodeData.mcIndex & (1<<i) ) printf( "1" ); else printf( "0" );
printf( "\n" );
return 0;
}
}
else
{
int o,i1,i2;
Cube::FactorEdgeIndex(finestIndex,o,i1,i2);
int d,off[3];
finest->depthAndOffset(d,off);
ri.node=finest;
ri.edgeIndex=finestIndex;
int offset,eIndex[2];
offset = BinaryNode< Real >::CenterIndex( d , off[o] );
switch(o){
case 0:
eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
break;
case 1:
eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
break;
case 2:
eIndex[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
eIndex[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
break;
}
ri.key= (long long)(o) | (long long)(eIndex[0])<<5 | (long long)(eIndex[1])<<25 | (long long)(offset)<<45;
return 1;
}
}
template<int Degree>
int Octree<Degree>::GetRootPair(const RootInfo& ri,int maxDepth,RootInfo& pair){
const TreeOctNode* node=ri.node;
int c1,c2,c;
Cube::EdgeCorners(ri.edgeIndex,c1,c2);
while(node->parent){
c=int(node-node->parent->children);
if(c!=c1 && c!=c2){return 0;}
if(!MarchingCubes::HasEdgeRoots(node->parent->nodeData.mcIndex,ri.edgeIndex)){
if(c==c1){return GetRootIndex(&node->parent->children[c2],ri.edgeIndex,maxDepth,pair);}
else{return GetRootIndex(&node->parent->children[c1],ri.edgeIndex,maxDepth,pair);}
}
node=node->parent;
}
return 0;
}
template<int Degree>
int Octree< Degree >::GetRootIndex( const RootInfo& ri , RootData& rootData , CoredPointIndex& index )
{
long long key = ri.key;
auto rootIter = rootData.boundaryRoots.find( key );
if( rootIter!=rootData.boundaryRoots.end() )
{
index.inCore = 1;
index.index = rootIter->second;
return 1;
}
else if( rootData.interiorRoots )
{
int eIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ];
if( rootData.edgesSet[ eIndex ] )
{
index.inCore = 0;
index.index = rootData.interiorRoots[ eIndex ];
return 1;
}
}
return 0;
}
template< int Degree >
int Octree< Degree >::SetMCRootPositions( TreeOctNode* node , int sDepth , Real isoValue , TreeOctNode::ConstNeighborKey5& neighborKey5 , RootData& rootData ,
std::vector< Point3D< float > >* interiorPositions , CoredMeshData* mesh , const Real* metSolution , int nonLinearFit )
{
Point3D< Real > position;
int eIndex;
RootInfo ri;
int count=0;
if( !MarchingCubes::HasRoots( node->nodeData.mcIndex ) ) return 0;
for( int i=0 ; i<DIMENSION ; i++ ) for( int j=0 ; j<2 ; j++ ) for( int k=0 ; k<2 ; k++ )
{
long long key;
eIndex = Cube::EdgeIndex( i , j , k );
if( GetRootIndex( node , eIndex , fData.depth , ri ) )
{
key = ri.key;
if( !rootData.interiorRoots || IsBoundaryEdge( node , i , j , k , sDepth ) )
{
std::unordered_map< long long , int >::iterator iter , end;
// Check if the root has already been set
#pragma omp critical (boundary_roots_hash_access)
{
iter = rootData.boundaryRoots.find( key );
end = rootData.boundaryRoots.end();
}
if( iter==end )
{
// Get the root information
GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit );
// Add the root if it hasn't been added already
#pragma omp critical (boundary_roots_hash_access)
{
iter = rootData.boundaryRoots.find( key );
end = rootData.boundaryRoots.end();
if( iter==end )
{
mesh->inCorePoints.push_back( position );
rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() ) - 1;
}
}
if( iter==end ) count++;
}
}
else
{
int nodeEdgeIndex = rootData.edgeIndices( ri.node )[ ri.edgeIndex ];
if( !rootData.edgesSet[ nodeEdgeIndex ] )
{
// Get the root information
GetRoot( ri , isoValue , neighborKey5 , position , rootData , sDepth , metSolution , nonLinearFit );
// Add the root if it hasn't been added already
#pragma omp critical (add_point_access)
{
if( !rootData.edgesSet[ nodeEdgeIndex ] )
{
rootData.interiorRoots[ nodeEdgeIndex ] = mesh->addOutOfCorePoint( position );
interiorPositions->push_back( position );
rootData.edgesSet[ nodeEdgeIndex ] = 1;
count++;
}
}
}
}
}
}
return count;
}
template<int Degree>
int Octree< Degree >::SetBoundaryMCRootPositions( int sDepth , Real isoValue , RootData& rootData , CoredMeshData* mesh , int nonLinearFit )
{
Point3D< Real > position;
int i,j,k,eIndex,hits=0;
RootInfo ri;
int count=0;
TreeOctNode* node;
node = tree.nextLeaf();
while( node )
{
if( MarchingCubes::HasRoots( node->nodeData.mcIndex ) )
{
hits=0;
for( i=0 ; i<DIMENSION ; i++ )
for( j=0 ; j<2 ; j++ )
for( k=0 ; k<2 ; k++ )
if( IsBoundaryEdge( node , i , j , k , sDepth ) )
{
hits++;
long long key;
eIndex = Cube::EdgeIndex( i , j , k );
if( GetRootIndex( node , eIndex , fData.depth , ri ) )
{
key = ri.key;
if( rootData.boundaryRoots.find(key)==rootData.boundaryRoots.end() )
{
GetRoot( ri , isoValue , position , rootData , sDepth , nonLinearFit );
mesh->inCorePoints.push_back( position );
rootData.boundaryRoots[key] = int( mesh->inCorePoints.size() )-1;
count++;
}
}
}
}
if( hits ) node=tree.nextLeaf(node);
else node=tree.nextBranch(node);
}
return count;
}
template<int Degree>
void Octree< Degree >::GetMCIsoEdges( TreeOctNode* node , int sDepth , std::vector< std::pair< RootInfo , RootInfo > >& edges )
{
TreeOctNode* temp;
int count=0 , tris=0;
int isoTri[ DIMENSION * MarchingCubes::MAX_TRIANGLES ];
FaceEdgesFunction fef;
int ref , fIndex;
std::unordered_map< long long , std::pair< RootInfo , int > > vertexCount;
fef.edges = &edges;
fef.maxDepth = fData.depth;
fef.vertexCount = &vertexCount;
count = MarchingCubes::AddTriangleIndices( node->nodeData.mcIndex , isoTri );
for( fIndex=0 ; fIndex<Cube::NEIGHBORS ; fIndex++ )
{
ref = Cube::FaceReflectFaceIndex( fIndex , fIndex );
fef.fIndex = ref;
temp = node->faceNeighbor( fIndex );
// If the face neighbor exists and has higher resolution than the current node,
// get the iso-curve from the neighbor
if( temp && temp->children && !IsBoundaryFace( node , fIndex , sDepth ) ) temp->processNodeFaces( temp , &fef , ref );
// Otherwise, get it from the node
else
{
RootInfo ri1 , ri2;
for( int j=0 ; j<count ; j++ )
for( int k=0 ; k<3 ; k++ )
if( fIndex==Cube::FaceAdjacentToEdges( isoTri[j*3+k] , isoTri[j*3+((k+1)%3)] ) )
if( GetRootIndex( node , isoTri[j*3+k] , fData.depth , ri1 ) && GetRootIndex( node , isoTri[j*3+((k+1)%3)] , fData.depth , ri2 ) )
{
long long key1 = ri1.key , key2 = ri2.key;
edges.push_back( std::pair< RootInfo , RootInfo >( ri1 , ri2 ) );
if( vertexCount.count( key1 )==0 )
{
vertexCount[key1].first = ri1;
vertexCount[key1].second = 0;
}
if( vertexCount.count( key2 )==0 )
{
vertexCount[key2].first = ri2;
vertexCount[key2].second = 0;
}
vertexCount[key1].second++;
vertexCount[key2].second--;
}
else
{
int r1 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+k] );
int r2 = MarchingCubes::HasEdgeRoots( node->nodeData.mcIndex , isoTri[j*3+((k+1)%3)] );
fprintf( stderr , "Bad Edge 2: %lld %lld\t%d %d\n" , ri1.key , ri2.key , r1 , r2 );
}
}
}
for( int i=0 ; i<int(edges.size()) ; i++ )
{
if( vertexCount.count( edges[i].first.key )==0 ) printf( "Could not find vertex: %lld\n" , edges[i].first.key );
else if( vertexCount[ edges[i].first.key ].second )
{
RootInfo ri;
GetRootPair( vertexCount[edges[i].first.key].first , fData.depth , ri );
long long key = ri.key;
if( vertexCount.count( key )==0 )
{
int d , off[3];
node->depthAndOffset( d , off );
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] );
}
else
{
edges.push_back( std::pair< RootInfo , RootInfo >( ri , edges[i].first ) );
vertexCount[ key ].second++;
vertexCount[ edges[i].first.key ].second--;
}
}
if( vertexCount.count( edges[i].second.key )==0 ) printf( "Could not find vertex: %lld\n" , edges[i].second.key );
else if( vertexCount[edges[i].second.key].second )
{
RootInfo ri;
GetRootPair( vertexCount[edges[i].second.key].first , fData.depth , ri );
long long key = ri.key;
if( vertexCount.count( key ) )
{
int d , off[3];
node->depthAndOffset( d , off );
printf( "Vertex pair not in list 2\t[%d] (%d %d %d)\n" , d , off[0] , off[1] , off[2] );
}
else
{
edges.push_back( std::pair< RootInfo , RootInfo >( edges[i].second , ri ) );
vertexCount[key].second--;
vertexCount[ edges[i].second.key ].second++;
}
}
}
}
template<int Degree>
#if MISHA_DEBUG
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 )
#else // !MISHA_DEBUG
int Octree< Degree >::GetMCIsoTriangles( TreeOctNode* node , CoredMeshData* mesh , RootData& rootData , std::vector< Point3D< float > >* interiorPositions , int offSet , int sDepth , bool addBarycenter , bool polygonMesh )
#endif // MISHA_DEBUG
{
int tris=0;
std::vector< std::pair< RootInfo , RootInfo > > edges;
std::vector< std::vector< std::pair< RootInfo , RootInfo > > > edgeLoops;
GetMCIsoEdges( node , sDepth , edges );
GetEdgeLoops( edges , edgeLoops );
for( int i=0 ; i<int(edgeLoops.size()) ; i++ )
{
CoredPointIndex p;
std::vector<CoredPointIndex> edgeIndices;
for( int j=0 ; j<int(edgeLoops[i].size()) ; j++ )
{
if( !GetRootIndex( edgeLoops[i][j].first , rootData , p ) ) printf( "Bad Point Index\n" );
else edgeIndices.push_back( p );
}
#if MISHA_DEBUG
tris += AddTriangles( mesh , edgeIndices , interiorPositions , offSet , polygonMesh , barycenters );
#else // !MISHA_DEBUG
tris += AddTriangles( mesh , edgeIndices , interiorPositions , offSet , addBarycenter , polygonMesh );
#endif // MISHA_DEBUG
}
return tris;
}
template< int Degree >
int Octree< Degree >::GetEdgeLoops( std::vector< std::pair< RootInfo , RootInfo > >& edges , std::vector< std::vector< std::pair< RootInfo , RootInfo > > >& loops )
{
int loopSize=0;
long long frontIdx , backIdx;
std::pair< RootInfo , RootInfo > e , temp;
loops.clear();
while( edges.size() )
{
std::vector< std::pair< RootInfo , RootInfo > > front , back;
e = edges[0];
loops.resize( loopSize+1 );
edges[0] = edges.back();
edges.pop_back();
frontIdx = e.second.key;
backIdx = e.first.key;
for( int j=int(edges.size())-1 ; j>=0 ; j-- )
{
if( edges[j].first.key==frontIdx || edges[j].second.key==frontIdx )
{
if( edges[j].first.key==frontIdx ) temp = edges[j];
else temp.first = edges[j].second , temp.second = edges[j].first;
frontIdx = temp.second.key;
front.push_back(temp);
edges[j] = edges.back();
edges.pop_back();
j = int(edges.size());
}
else if( edges[j].first.key==backIdx || edges[j].second.key==backIdx )
{
if( edges[j].second.key==backIdx ) temp = edges[j];
else temp.first = edges[j].second , temp.second = edges[j].first;
backIdx = temp.first.key;
back.push_back(temp);
edges[j] = edges.back();
edges.pop_back();
j = int(edges.size());
}
}
for( int j=int(back.size())-1 ; j>=0 ; j-- ) loops[loopSize].push_back( back[j] );
loops[loopSize].push_back(e);
for( int j=0 ; j<int(front.size()) ; j++ ) loops[loopSize].push_back( front[j] );
loopSize++;
}
return int(loops.size());
}
template<int Degree>
#if MISHA_DEBUG
int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool polygonMesh , std::vector< Point3D< float > >* barycenters )
#else // !MISHA_DEBUG
int Octree<Degree>::AddTriangles( CoredMeshData* mesh , std::vector<CoredPointIndex>& edges , std::vector<Point3D<float> >* interiorPositions , int offSet , bool addBarycenter , bool polygonMesh )
#endif // MISHA_DEBUG
{
MinimalAreaTriangulation< float > MAT;
std::vector< Point3D< float > > vertices;
std::vector< TriangleIndex > triangles;
if( polygonMesh )
{
std::vector< CoredVertexIndex > vertices( edges.size() );
for( int i=0 ; i<int(edges.size()) ; i++ )
{
vertices[i].idx = edges[i].index;
vertices[i].inCore = (edges[i].inCore!=0);
}
#pragma omp critical (add_polygon_access)
{
mesh->addPolygon( vertices );
}
return 1;
}
if( edges.size()>3 )
{
bool isCoplanar = false;
#if MISHA_DEBUG
if( barycenters )
#else // !MISHA_DEBUG
if( addBarycenter )
#endif // MISHA_DEBUG
for( int i=0 ; i<int(edges.size()) ; i++ )
for( int j=0 ; j<i ; j++ )
if( (i+1)%edges.size()!=j && (j+1)%edges.size()!=i )
{
Point3D< Real > v1 , v2;
if( edges[i].inCore ) v1 = mesh->inCorePoints[ edges[i].index ];
else v1 = (*interiorPositions)[ edges[i].index-offSet ];
if( edges[j].inCore ) v2 = mesh->inCorePoints[ edges[j].index ];
else v2 = (*interiorPositions)[ edges[j].index-offSet ];
for( int k=0 ; k<3 ; k++ ) if( v1[k]==v2[k] ) isCoplanar = true;
}
if( isCoplanar )
{
Point3D< Real > c;
c[0] = c[1] = c[2] = 0;
for( int i=0 ; i<int(edges.size()) ; i++ )
{
Point3D<Real> p;
if(edges[i].inCore) p = mesh->inCorePoints[edges[i].index ];
else p = (*interiorPositions)[edges[i].index-offSet];
c += p;
}
c /= Real( edges.size() );
int cIdx;
#pragma omp critical (add_point_access)
{
cIdx = mesh->addOutOfCorePoint( c );
#if MISHA_DEBUG
barycenters->push_back( c );
#else // !MISHA_DEBUG
interiorPositions->push_back( c );
#endif // MISHA_DEBUG
}
for( int i=0 ; i<int(edges.size()) ; i++ )
{
std::vector< CoredVertexIndex > vertices( 3 );
vertices[0].idx = edges[i ].index;
vertices[1].idx = edges[(i+1)%edges.size()].index;
vertices[2].idx = cIdx;
vertices[0].inCore = (edges[i ].inCore!=0);
vertices[1].inCore = (edges[(i+1)%edges.size()].inCore!=0);
vertices[2].inCore = 0;
#pragma omp critical (add_polygon_access)
{
mesh->addPolygon( vertices );
}
}
return int( edges.size() );
}
else
{
vertices.resize( edges.size() );
// Add the points
for( int i=0 ; i<int(edges.size()) ; i++ )
{
Point3D< Real > p;
if( edges[i].inCore ) p = mesh->inCorePoints[edges[i].index ];
else p = (*interiorPositions)[edges[i].index-offSet];
vertices[i] = p;
}
MAT.GetTriangulation( vertices , triangles );
for( int i=0 ; i<int(triangles.size()) ; i++ )
{
std::vector< CoredVertexIndex > _vertices( 3 );
for( int j=0 ; j<3 ; j++ )
{
_vertices[j].idx = edges[ triangles[i].idx[j] ].index;
_vertices[j].inCore = (edges[ triangles[i].idx[j] ].inCore!=0);
}
#pragma omp critical (add_polygon_access)
{
mesh->addPolygon( _vertices );
}
}
}
}
else if( edges.size()==3 )
{
std::vector< CoredVertexIndex > vertices( 3 );
for( int i=0 ; i<3 ; i++ )
{
vertices[i].idx = edges[i].index;
vertices[i].inCore = (edges[i].inCore!=0);
}
#pragma omp critical (add_polygon_access)
mesh->addPolygon( vertices );
}
return int(edges.size())-2;
}
template< int Degree >
Real* Octree< Degree >::GetSolutionGrid( int& res , float isoValue , int depth )
{
if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth();
BSplineData< Degree , Real > fData;
fData.set( depth );
fData.setValueTables( fData.VALUE_FLAG );
res = 1<<depth;
Real* values = new float[ res * res * res ];
memset( values , 0 , sizeof( float ) * res * res * res );
for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode( n ) )
{
if( n->d>depth ) continue;
if( n->d<_minDepth ) continue;
int d , idx[3] , start[3] , end[3];
n->depthAndOffset( d , idx );
for( int i=0 ; i<3 ; i++ )
{
// Get the index of the functions
idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] );
// Figure out which samples fall into the range
fData.setSampleSpan( idx[i] , start[i] , end[i] );
// We only care about the odd indices
if( !(start[i]&1) ) start[i]++;
if( !( end[i]&1) ) end[i]--;
}
Real coefficient = n->nodeData.solution;
for( int x=start[0] ; x<=end[0] ; x+=2 )
for( int y=start[1] ; y<=end[1] ; y+=2 )
for( int z=start[2] ; z<=end[2] ; z+=2 )
{
int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1;
values[ zz*res*res + yy*res + xx ] +=
coefficient *
fData.valueTables[ idx[0]+x*fData.functionCount ] *
fData.valueTables[ idx[1]+y*fData.functionCount ] *
fData.valueTables[ idx[2]+z*fData.functionCount ];
}
}
for( int i=0 ; i<res*res*res ; i++ ) values[i] -= isoValue;
return values;
}
template< int Degree >
Real* Octree< Degree >::GetWeightGrid( int& res , int depth )
{
if( depth<=0 || depth>tree.maxDepth() ) depth = tree.maxDepth();
res = 1<<tree.maxDepth();
Real* values = new float[ res * res * res ];
memset( values , 0 , sizeof( float ) * res * res * res );
for( TreeOctNode* n=tree.nextNode() ; n ; n=tree.nextNode( n ) )
{
if( n->d>depth ) continue;
int d , idx[3] , start[3] , end[3];
n->depthAndOffset( d , idx );
for( int i=0 ; i<3 ; i++ )
{
// Get the index of the functions
idx[i] = BinaryNode< double >::CenterIndex( d , idx[i] );
// Figure out which samples fall into the range
fData.setSampleSpan( idx[i] , start[i] , end[i] );
// We only care about the odd indices
if( !(start[i]&1) ) start[i]++;
if( !( end[i]&1) ) end[i]--;
}
for( int x=start[0] ; x<=end[0] ; x+=2 )
for( int y=start[1] ; y<=end[1] ; y+=2 )
for( int z=start[2] ; z<=end[2] ; z+=2 )
{
int xx = (x-1)>>1 , yy = (y-1)>>1 , zz = (z-1)>>1;
values[ zz*res*res + yy*res + xx ] +=
n->nodeData.centerWeightContribution *
fData.valueTables[ idx[0]+x*fData.functionCount ] *
fData.valueTables[ idx[1]+y*fData.functionCount ] *
fData.valueTables[ idx[2]+z*fData.functionCount ];
}
}
return values;
}
////////////////
// VertexData //
////////////////
long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth){
int idx[DIMENSION];
return CenterIndex(node,maxDepth,idx);
}
long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth,int idx[DIMENSION]){
int d,o[3];
node->depthAndOffset(d,o);
for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
long long VertexData::CenterIndex(int depth,const int offSet[DIMENSION],int maxDepth,int idx[DIMENSION]){
for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,depth+1,offSet[i]<<1,1);}
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
long long VertexData::CornerIndex(const TreeOctNode* node,int cIndex,int maxDepth){
int idx[DIMENSION];
return CornerIndex(node,cIndex,maxDepth,idx);
}
long long VertexData::CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int idx[DIMENSION] )
{
int x[DIMENSION];
Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] );
int d , o[3];
node->depthAndOffset( d , o );
for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , d , o[i] , x[i] );
return CornerIndexKey( idx );
}
long long VertexData::CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int idx[DIMENSION] )
{
int x[DIMENSION];
Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] );
for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode<Real>::CornerIndex( maxDepth+1 , depth , offSet[i] , x[i] );
return CornerIndexKey( idx );
}
long long VertexData::CornerIndexKey( const int idx[DIMENSION] )
{
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth){
int idx[DIMENSION];
return FaceIndex(node,fIndex,maxDepth,idx);
}
long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth,int idx[DIMENSION]){
int dir,offset;
Cube::FactorFaceIndex(fIndex,dir,offset);
int d,o[3];
node->depthAndOffset(d,o);
for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
idx[dir]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,o[dir],offset);
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth){
int idx[DIMENSION];
return EdgeIndex(node,eIndex,maxDepth,idx);
}
long long VertexData::EdgeIndex(const TreeOctNode* node,int eIndex,int maxDepth,int idx[DIMENSION]){
int o,i1,i2;
int d,off[3];
node->depthAndOffset(d,off);
for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode<Real>::CornerIndex(maxDepth+1,d+1,off[i]<<1,1);}
Cube::FactorEdgeIndex(eIndex,o,i1,i2);
switch(o){
case 0:
idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i1);
idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
break;
case 1:
idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
idx[2]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[2],i2);
break;
case 2:
idx[0]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[0],i1);
idx[1]=BinaryNode<Real>::CornerIndex(maxDepth+1,d,off[1],i2);
break;
};
return (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
}
}
pcl::PointCloud::size
std::size_t size() const
Definition point_cloud.h:443
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
pcl::poisson::BSplineData
Definition bspline_data.h:46
pcl::poisson::BSplineData::functionCount
int functionCount
Definition bspline_data.h:65
pcl::poisson::BSplineData::set
void set(int maxDepth, bool useDotRatios=true, bool reflectBoundary=false)
Definition bspline_data.hpp:116
pcl::poisson::BSplineData::SymmetricIndex
static int SymmetricIndex(int i1, int i2)
Definition bspline_data.hpp:381
pcl::poisson::BSplineData::setValueTables
virtual void setValueTables(int flags, double smooth=0)
Definition bspline_data.hpp:322
pcl::poisson::BSplineData::setSampleSpan
void setSampleSpan(int idx, int &start, int &end, double smooth=0) const
Definition bspline_data.hpp:303
pcl::poisson::BSplineData::valueTables
Real * valueTables
Definition bspline_data.h:67
pcl::poisson::BSplineData::VALUE_FLAG
static const int VALUE_FLAG
Definition bspline_data.h:62
pcl::poisson::BinaryNode::Index
static int Index(int depth, int offSet)
Definition binary_node.h:46
pcl::poisson::BinaryNode::CornerIndex
static int CornerIndex(int depth, int offSet)
Definition binary_node.h:48
pcl::poisson::BinaryNode::CenterIndex
static int CenterIndex(int depth, int offSet)
Definition binary_node.h:47
pcl::poisson::CoredMeshData
Definition geometry.h:200
pcl::poisson::CoredMeshData::outOfCorePointCount
virtual int outOfCorePointCount(void)=0
pcl::poisson::Cube::FaceReflectEdgeIndex
static int FaceReflectEdgeIndex(int idx, int faceIndex)
pcl::poisson::Cube::FaceAdjacentToEdges
static int FaceAdjacentToEdges(int eIndex1, int eIndex2)
pcl::poisson::Cube::FactorEdgeIndex
static void FactorEdgeIndex(int idx, int &orientation, int &i, int &j)
pcl::poisson::Cube::FactorCornerIndex
static void FactorCornerIndex(int idx, int &x, int &y, int &z)
pcl::poisson::Cube::FacesAdjacentToEdge
static void FacesAdjacentToEdge(int eIndex, int &f1Index, int &f2Index)
pcl::poisson::Cube::FaceReflectFaceIndex
static int FaceReflectFaceIndex(int idx, int faceIndex)
pcl::poisson::Cube::FaceCorners
static void FaceCorners(int idx, int &c1, int &c2, int &c3, int &c4)
pcl::poisson::Cube::AntipodalCornerIndex
static int AntipodalCornerIndex(int idx)
pcl::poisson::Cube::CornerIndex
static int CornerIndex(int x, int y, int z)
pcl::poisson::Cube::EdgeIndex
static int EdgeIndex(int orientation, int i, int j)
pcl::poisson::Cube::EdgeReflectEdgeIndex
static int EdgeReflectEdgeIndex(int edgeIndex)
pcl::poisson::Cube::EDGES
@ EDGES
Definition marching_cubes_poisson.h:59
pcl::poisson::Cube::NEIGHBORS
@ NEIGHBORS
Definition marching_cubes_poisson.h:59
pcl::poisson::Cube::CORNERS
@ CORNERS
Definition marching_cubes_poisson.h:59
pcl::poisson::Cube::FactorFaceIndex
static void FactorFaceIndex(int idx, int &x, int &y, int &z)
pcl::poisson::Cube::EdgeCorners
static void EdgeCorners(int idx, int &c1, int &c2)
pcl::poisson::EdgeIndex
Definition geometry.h:138
pcl::poisson::MarchingCubes::cornerMap
static const int * cornerMap()
pcl::poisson::MarchingCubes::AddTriangleIndices
static int AddTriangleIndices(int mcIndex, int *triangles)
pcl::poisson::MarchingCubes::HasEdgeRoots
static int HasEdgeRoots(int mcIndex, int edgeIndex)
pcl::poisson::MarchingCubes::MAX_TRIANGLES
@ MAX_TRIANGLES
Definition marching_cubes_poisson.h:112
pcl::poisson::MarchingCubes::HasRoots
static int HasRoots(const double v[Cube::CORNERS], double isoValue)
pcl::poisson::MarchingCubes::GetIndex
static int GetIndex(const double values[Cube::CORNERS], double iso)
pcl::poisson::MarchingCubes::edgeMask
static const int * edgeMask()
pcl::poisson::OctNode::ConstNeighborKey3
Definition octree_poisson.h:223
pcl::poisson::OctNode::ConstNeighborKey3::neighbors
ConstNeighbors3 * neighbors
Definition octree_poisson.h:225
pcl::poisson::OctNode::ConstNeighbors3::neighbors
const OctNode * neighbors[3][3][3]
Definition octree_poisson.h:218
pcl::poisson::OctNode< class TreeNodeData, Real >
pcl::poisson::OctNode::d
short d
Definition octree_poisson.h:94
pcl::poisson::OctNode::depthAndOffset
void depthAndOffset(int &depth, int offset[DIMENSION]) const
Definition octree_poisson.hpp:129
pcl::poisson::OctNode::maxDepth
int maxDepth(void) const
Definition octree_poisson.hpp:181
pcl::poisson::OctNode::depth
int depth(void) const
Definition octree_poisson.hpp:137
pcl::poisson::OctNode::nodes
int nodes(void) const
Definition octree_poisson.hpp:194
pcl::poisson::OctNode< class TreeNodeData, Real >::CornerIndex
static int CornerIndex(const Point3D< Real > &center, const Point3D< Real > &p)
Definition octree_poisson.hpp:797
pcl::poisson::OctNode::off
short off[DIMENSION]
Definition octree_poisson.h:94
pcl::poisson::OctNode::setFullDepth
void setFullDepth(int maxDepth)
Definition octree_poisson.hpp:80
pcl::poisson::OctNode::nextNode
const OctNode * nextNode(const OctNode *currentNode=NULL) const
Definition octree_poisson.hpp:289
pcl::poisson::OctNode::centerAndWidth
void centerAndWidth(Point3D< Real > &center, Real &width) const
Definition octree_poisson.hpp:149
pcl::poisson::OctNode::nextLeaf
const OctNode * nextLeaf(const OctNode *currentLeaf=NULL) const
Definition octree_poisson.hpp:263
pcl::poisson::OctNode::parent
OctNode * parent
Definition octree_poisson.h:92
pcl::poisson::OctNode::children
OctNode * children
Definition octree_poisson.h:93
pcl::poisson::OctNode< class TreeNodeData, Real >::ProcessFixedDepthNodeAdjacentNodes
static void ProcessFixedDepthNodeAdjacentNodes(int maxDepth, OctNode *node1, int width1, OctNode *node2, int width2, int depth, NodeAdjacencyFunction *F, int processCurrent=1)
pcl::poisson::OctNode< class TreeNodeData, Real >::Index
static void Index(int depth, const int offset[3], short &d, short off[DIMENSION])
Definition octree_poisson.hpp:121
pcl::poisson::OctNode::nodeData
NodeData nodeData
Definition octree_poisson.h:95
pcl::poisson::OctNode::faceNeighbor
OctNode * faceNeighbor(int faceIndex, int forceChildren=0)
Definition octree_poisson.hpp:908
pcl::poisson::OctNode::initChildren
int initChildren(void)
Definition octree_poisson.hpp:89
pcl::poisson::Octree
Definition multi_grid_octree_data.h:196
pcl::poisson::Octree::RefineBoundary
void RefineBoundary(int subdivisionDepth)
Definition multi_grid_octree_data.hpp:2381
pcl::poisson::Octree::setBSplineData
void setBSplineData(int maxDepth, Real normalSmooth=-1, bool reflectBoundary=false)
Definition multi_grid_octree_data.hpp:981
pcl::poisson::Octree::MemoryUsage
static double MemoryUsage(void)
Definition multi_grid_octree_data.hpp:521
pcl::poisson::Octree::LaplacianMatrixIteration
int LaplacianMatrixIteration(int subdivideDepth, bool showResidual, int minIters, double accuracy)
Definition multi_grid_octree_data.hpp:1901
pcl::poisson::Octree::GetIsoValue
Real GetIsoValue(void)
Definition multi_grid_octree_data.hpp:2817
pcl::poisson::Octree::Octree
Octree(void)
Definition multi_grid_octree_data.hpp:528
pcl::poisson::Octree::GetSolutionGrid
Real * GetSolutionGrid(int &res, float isoValue=0.f, int depth=-1)
Definition multi_grid_octree_data.hpp:3804
pcl::poisson::Octree::SetLaplacianConstraints
void SetLaplacianConstraints(void)
Definition multi_grid_octree_data.hpp:2137
pcl::poisson::Octree::finalize
void finalize(void)
Definition multi_grid_octree_data.hpp:990
pcl::poisson::Octree::GetWeightGrid
Real * GetWeightGrid(int &res, int depth=-1)
Definition multi_grid_octree_data.hpp:3849
pcl::poisson::Octree::ClipTree
void ClipTree(void)
Definition multi_grid_octree_data.hpp:2123
pcl::poisson::Octree::setTree
int setTree(typename pcl::PointCloud< PointNT >::ConstPtr input_, int maxDepth, int minDepth, int kernelDepth, Real samplesPerNode, Real scaleFactor, Point3D< Real > &center, Real &scale, int useConfidence, Real constraintWeight, bool adaptiveWeights)
Definition multi_grid_octree_data.hpp:763
pcl::poisson::PoissonOpenMPException
An exception that is thrown when something goes wrong inside an openMP for loop.
Definition poisson_exceptions.h:131
pcl::poisson::SortedTreeNodes
Definition multi_grid_octree_data.h:115
pcl::poisson::SortedTreeNodes::setEdgeTable
void setEdgeTable(EdgeTableData &eData, const TreeOctNode *rootNode, int depth, int threads)
Definition multi_grid_octree_data.hpp:307
pcl::poisson::SortedTreeNodes::maxDepth
int maxDepth
Definition multi_grid_octree_data.h:119
pcl::poisson::SortedTreeNodes::getMaxEdgeCount
int getMaxEdgeCount(const TreeOctNode *rootNode, int depth, int threads) const
Definition multi_grid_octree_data.hpp:437
pcl::poisson::SortedTreeNodes::getMaxCornerCount
int getMaxCornerCount(const TreeOctNode *rootNode, int depth, int maxDepth, int threads) const
Definition multi_grid_octree_data.hpp:250
pcl::poisson::SortedTreeNodes::treeNodes
TreeOctNode ** treeNodes
Definition multi_grid_octree_data.h:117
pcl::poisson::SortedTreeNodes::SortedTreeNodes
SortedTreeNodes(void)
Definition multi_grid_octree_data.hpp:73
pcl::poisson::SortedTreeNodes::~SortedTreeNodes
~SortedTreeNodes(void)
Definition multi_grid_octree_data.hpp:79
pcl::poisson::SortedTreeNodes::set
void set(TreeOctNode &root)
Definition multi_grid_octree_data.hpp:86
pcl::poisson::SortedTreeNodes::nodeCount
int * nodeCount
Definition multi_grid_octree_data.h:118
pcl::poisson::SortedTreeNodes::setCornerTable
void setCornerTable(CornerTableData &cData, const TreeOctNode *rootNode, int depth, int threads) const
Definition multi_grid_octree_data.hpp:111
pcl::poisson::SparseMatrix
Definition sparse_matrix.h:54
pcl::poisson::SparseMatrix::rowSizes
int * rowSizes
Definition sparse_matrix.h:65
pcl::poisson::SparseMatrix::SetAllocator
static void SetAllocator(int blockSize)
Definition sparse_matrix.hpp:56
pcl::poisson::SparseMatrix::Resize
void Resize(int rows)
Definition sparse_matrix.hpp:160
pcl::poisson::SparseMatrix::SetRowSize
void SetRowSize(int row, int count)
Definition sparse_matrix.hpp:206
pcl::poisson::SparseMatrix::rows
int rows
Definition sparse_matrix.h:64
pcl::poisson::SparseMatrix::Entries
int Entries(void) const
Definition sparse_matrix.hpp:93
pcl::poisson::SparseMatrix::internalAllocator
static Allocator< MatrixEntry< T > > internalAllocator
Definition sparse_matrix.h:60
pcl::poisson::SparseSymmetricMatrix
Definition sparse_matrix.h:146
pcl::poisson::SparseSymmetricMatrix::Solve
static int Solve(const SparseSymmetricMatrix< T > &M, const Vector< T2 > &b, int iters, Vector< T2 > &solution, T2 eps=1e-8, int reset=1, int threads=0, bool addDCTerm=false, bool solveNormal=false)
Definition sparse_matrix.hpp:852
pcl::poisson::Square::CornerIndex
static int CornerIndex(int x, int y)
pcl::poisson::Square::CORNERS
@ CORNERS
Definition marching_cubes_poisson.h:43
pcl::poisson::Square::AntipodalCornerIndex
static int AntipodalCornerIndex(int idx)
pcl::poisson::Square::FactorCornerIndex
static void FactorCornerIndex(int idx, int &x, int &y)
pcl::poisson::TreeNodeData::mcIndex
int mcIndex
Definition multi_grid_octree_data.h:180
pcl::poisson::TreeNodeData::TreeNodeData
TreeNodeData(void)
Definition multi_grid_octree_data.hpp:497
pcl::poisson::TreeNodeData::solution
Real solution
Definition multi_grid_octree_data.h:187
pcl::poisson::TreeNodeData::pointIndex
int pointIndex
Definition multi_grid_octree_data.h:188
pcl::poisson::TreeNodeData::normalIndex
int normalIndex
Definition multi_grid_octree_data.h:184
pcl::poisson::TreeNodeData::centerWeightContribution
Real centerWeightContribution
Definition multi_grid_octree_data.h:183
pcl::poisson::TreeNodeData::nodeIndex
int nodeIndex
Definition multi_grid_octree_data.h:177
pcl::poisson::TreeNodeData::constraint
Real constraint
Definition multi_grid_octree_data.h:187
pcl::poisson::TreeNodeData::~TreeNodeData
~TreeNodeData(void)
Definition multi_grid_octree_data.hpp:509
pcl::poisson::TreeNodeData::UseIndex
static int UseIndex
Definition multi_grid_octree_data.h:176
pcl::poisson::Vector
Definition vector.h:42
pcl::poisson::Vector::Resize
void Resize(std::size_t N)
Definition vector.hpp:63
pcl::poisson::VertexData::CenterIndex
static long long CenterIndex(int depth, const int offSet[DIMENSION], int maxDepth, int index[DIMENSION])
Definition multi_grid_octree_data.hpp:3901
pcl::poisson::VertexData::CornerIndex
static long long CornerIndex(int depth, const int offSet[DIMENSION], int cIndex, int maxDepth, int index[DIMENSION])
Definition multi_grid_octree_data.hpp:3921
pcl::poisson::VertexData::FaceIndex
static long long FaceIndex(const TreeOctNode *node, int fIndex, int maxDepth, int index[DIMENSION])
Definition multi_grid_octree_data.hpp:3939
pcl::poisson::VertexData::CornerIndexKey
static long long CornerIndexKey(const int index[DIMENSION])
Definition multi_grid_octree_data.hpp:3929
pcl::poisson::VertexData::EdgeIndex
static long long EdgeIndex(const TreeOctNode *node, int eIndex, int maxDepth, int index[DIMENSION])
Definition multi_grid_octree_data.hpp:3954
pcl::B
@ B
Definition norms.h:54
pcl::device::dot
__device__ __forceinline__ float dot(const float3 &v1, const float3 &v2)
Definition utils.hpp:59
pcl::poisson::EPSILON
constexpr Real EPSILON
Definition multi_grid_octree_data.hpp:52
pcl::poisson::ROUND_EPS
constexpr Real ROUND_EPS
Definition multi_grid_octree_data.hpp:53
pcl::poisson::TreeOctNode
pcl::poisson::OctNode< class TreeNodeData, Real > TreeOctNode
Definition multi_grid_octree_data.h:87
pcl::poisson::Real
float Real
Definition multi_grid_octree_data.h:85
pcl::poisson::atomicOr
void atomicOr(volatile int &dest, int value)
Definition multi_grid_octree_data.hpp:55
pcl::poisson::Length
double Length(const Point3D< Real > &p)
Definition geometry.hpp:63
pcl::poisson::MATRIX_ENTRY_EPSILON
constexpr Real MATRIX_ENTRY_EPSILON
Definition multi_grid_octree_data.hpp:51
pcl
Definition convolution.h:46
pcl::poisson::Point3D
Definition geometry.h:51
pcl::poisson::SortedTreeNodes::CornerIndices
Definition multi_grid_octree_data.h:124
pcl::poisson::SortedTreeNodes::CornerTableData
Definition multi_grid_octree_data.h:131
pcl::poisson::SortedTreeNodes::CornerTableData::offsets
std::vector< int > offsets
Definition multi_grid_octree_data.h:141
pcl::poisson::SortedTreeNodes::CornerTableData::operator[]
CornerIndices & operator[](const TreeOctNode *node)
Definition multi_grid_octree_data.hpp:107
pcl::poisson::SortedTreeNodes::CornerTableData::cTable
std::vector< CornerIndices > cTable
Definition multi_grid_octree_data.h:140
pcl::poisson::SortedTreeNodes::CornerTableData::cCount
int cCount
Definition multi_grid_octree_data.h:139
pcl::poisson::SortedTreeNodes::CornerTableData::cornerIndices
CornerIndices & cornerIndices(const TreeOctNode *node)
Definition multi_grid_octree_data.hpp:109
pcl::poisson::SortedTreeNodes::EdgeIndices
Definition multi_grid_octree_data.h:148
pcl::poisson::SortedTreeNodes::EdgeTableData
Definition multi_grid_octree_data.h:155
pcl::poisson::SortedTreeNodes::EdgeTableData::eTable
std::vector< EdgeIndices > eTable
Definition multi_grid_octree_data.h:164
pcl::poisson::SortedTreeNodes::EdgeTableData::eCount
int eCount
Definition multi_grid_octree_data.h:163
pcl::poisson::SortedTreeNodes::EdgeTableData::offsets
std::vector< int > offsets
Definition multi_grid_octree_data.h:165
pcl::poisson::SortedTreeNodes::EdgeTableData::operator[]
EdgeIndices & operator[](const TreeOctNode *node)
Definition multi_grid_octree_data.hpp:303
pcl::poisson::SortedTreeNodes::EdgeTableData::edgeIndices
EdgeIndices & edgeIndices(const TreeOctNode *node)
Definition multi_grid_octree_data.hpp:305
pcl::poisson::UpSampleData
Definition multi_grid_octree_data.hpp:1503
pcl::poisson::UpSampleData::UpSampleData
UpSampleData(void)
Definition multi_grid_octree_data.hpp:1506
pcl::poisson::UpSampleData::UpSampleData
UpSampleData(int s, double v1, double v2)
Definition multi_grid_octree_data.hpp:1507
pcl::poisson::UpSampleData::start
int start
Definition multi_grid_octree_data.hpp:1504