1/*
2Copyright (c) 2006, Michael Kazhdan and Matthew Bolitho
3All rights reserved.
4
5Redistribution and use in source and binary forms, with or without modification,
6are permitted provided that the following conditions are met:
7
8Redistributions of source code must retain the above copyright notice, this list of
9conditions and the following disclaimer. Redistributions in binary form must reproduce
10the above copyright notice, this list of conditions and the following disclaimer
11in the documentation and/or other materials provided with the distribution.
12
13Neither the name of the Johns Hopkins University nor the names of its contributors
14may be used to endorse or promote products derived from this software without specific
15prior written permission.
16
17THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
18EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO THE IMPLIED WARRANTIES
19OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
20SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
21INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22TO, PROCUREMENT OF SUBSTITUTE  GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
23BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
25ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
26DAMAGE.
27*/
28
29#include "PointStream.h"
30
31#define ITERATION_POWER 1.0/3
32#define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12
33
34
35const double MATRIX_ENTRY_EPSILON = 0;
36const double EPSILON              = 1e-6;
37const double ROUND_EPS            = 1e-5;
38
39
40
41//////////////////
42// TreeNodeData //
43//////////////////
44size_t TreeNodeData::NodeCount = 0;
45TreeNodeData::TreeNodeData( void ){ nodeIndex = (int)NodeCount ; NodeCount++; }
46TreeNodeData::~TreeNodeData( void ) { }
47
48
49////////////
50// Octree //
51////////////
52template< class Real > double Octree< Real >::maxMemoryUsage=0;
53
54template< class Real >
55double Octree< Real >::MemoryUsage( void )
56{
57        double mem = double( MemoryInfo::Usage() ) / (1<<20);
58        if( mem>maxMemoryUsage ) maxMemoryUsage=mem;
59        return mem;
60}
61
62template< class Real >
63Octree< Real >::Octree( void )
64{
65        threads = 1;
66        _constrainValues = false;
67        _multigridDegree = 0;
68}
69template< class Real >
70template< int FEMDegree >
71void Octree< Real >::FunctionIndex( const TreeOctNode* node , int idx[3] )
72{
73        int d;
74        node->depthAndOffset( d , idx );
75        for( int dd=0 ; dd<DIMENSION ; dd++ ) idx[dd] = BSplineData< FEMDegree >::FunctionIndex( d-1 , idx[dd] );
76}
77
78template< class Real > bool Octree< Real >::_InBounds( Point3D< Real > p ) const { return p[0]>=Real(0.) && p[0]<=Real(1.0) && p[1]>=Real(0.) && p[1]<=Real(1.0) && p[2]>=Real(0.) && p[2]<=Real(1.0); }
79template< class Real >
80template< int FEMDegree >
81bool Octree< Real >::IsValidNode( const TreeOctNode* node , bool dirichlet  )
82{
83        if( !node || node->depth()<1 ) return false;
84        int d , off[3];
85        node->depthAndOffset( d , off );
86        int dim = BSplineData< FEMDegree >::Dimension( d-1 );
87        if( FEMDegree&1 && dirichlet ) return !( off[0]<=0 || off[1]<=0 || off[2]<=0 || off[0]>=dim-1 || off[1]>=dim-1 || off[2]>=dim-1 );
88        else                           return !( off[0]< 0 || off[1]< 0 || off[2]< 0 || off[0]> dim-1 || off[1]> dim-1 || off[2]> dim-1 );
89}
90template< class Real >
91void Octree< Real >::_SetFullDepth( TreeOctNode* node , int depth )
92{
93        if( node->depth()==0 || _Depth( node )<depth )
94        {
95                if( !node->children ) node->initChildren();
96                for( int c=0 ; c<Cube::CORNERS ; c++ ) _SetFullDepth( node->children+c , depth );
97        }
98}
99template< class Real >
100void Octree< Real >::_setFullDepth( int depth )
101{
102        if( !_tree.children ) _tree.initChildren();
103        for( int c=0 ; c<Cube::CORNERS ; c++ ) _SetFullDepth( _tree.children+c , depth );
104}
105template< class Real >
106template< class PointReal , int NormalDegree , int WeightDegree , int DataDegree , class Data , class _Data >
107int Octree< Real >::SetTree( OrientedPointStream< PointReal >* pointStream , int minDepth , int maxDepth , int fullDepth ,
108                                                        int splatDepth , Real samplesPerNode , Real scaleFactor ,
109                                                        bool useConfidence , bool useNormalWeights , Real constraintWeight , int adaptiveExponent ,
110                                                        SparseNodeData< Real , WeightDegree >& densityWeights , SparseNodeData< PointData< Real > , 0 >& pointInfo , SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo , SparseNodeData< Real , NormalDegree >& nodeWeights ,
111                                                        SparseNodeData< ProjectiveData< _Data > , DataDegree >* dataValues ,
112                                                        XForm4x4< Real >& xForm , bool dirichlet , bool makeComplete )
113{
114        OrientedPointStreamWithData< PointReal , Data >* pointStreamWithData = ( OrientedPointStreamWithData< PointReal , Data >* )pointStream;
115        _tree.initChildren() , _spaceRoot = _tree.children;
116        splatDepth = std::max< int >( 0 , std::min< int >( splatDepth , maxDepth ) );
117
118        _dirichlet = dirichlet;
119        _constrainValues = (constraintWeight>0);
120
121        XForm3x3< Real > xFormN;
122        for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) xFormN(i,j) = xForm(i,j);
123        xFormN = xFormN.transpose().inverse();
124        minDepth = std::max< int >( 0 , std::min< int >( minDepth , maxDepth ) ); // 0<=minDepth <= maxDepth
125        fullDepth = std::max< int >( minDepth , std::min< int >( fullDepth , maxDepth ) );      // minDepth <= fullDepth <= maxDepth
126
127#if 0
128        // For Neumann constraints, the function at depth 0 is constant so the system matrix is zero if there is no screening.
129        if( !_dirichlet && !_constrainValues ) minDepth = std::max< int >( minDepth , 1 );
130#endif
131        minDepth++ , maxDepth++;
132
133        _minDepth = minDepth;
134        _fullDepth = fullDepth;
135        _splatDepth = splatDepth;
136        double pointWeightSum = 0;
137        int i , cnt=0;
138
139        PointSupportKey< WeightDegree > weightKey;
140        PointSupportKey< DataDegree > dataKey;
141        PointSupportKey< NormalDegree > normalKey;
142        weightKey.set( maxDepth ) , dataKey.set( maxDepth ) , normalKey.set( maxDepth );
143
144        _setFullDepth( _fullDepth );
145
146        // Read through once to get the center and scale
147        {
148                Point3D< Real > min , max;
149                double t = Time();
150                Point3D< Real > p;
151                OrientedPoint3D< PointReal > _p;
152                while( pointStream->nextPoint( _p ) )
153                {
154                        p = xForm * Point3D< Real >(_p.p);
155                        for( i=0 ; i<DIMENSION ; i++ )
156                        {
157                                if( !cnt || p[i]<min[i] ) min[i] = p[i];
158                                if( !cnt || p[i]>max[i] ) max[i] = p[i];
159                        }
160                        cnt++;
161                }
162
163                _scale = std::max< Real >( max[0]-min[0] , std::max< Real >( max[1]-min[1] , max[2]-min[2] ) );
164                _center = ( max+min ) /2;
165        }
166
167        _scale *= scaleFactor;
168        for( i=0 ; i<DIMENSION ; i++ ) _center[i] -= _scale / 2;
169
170        // Update the transformation
171        {
172                XForm4x4< Real > trans = XForm4x4< Real >::Identity() , scale = XForm4x4< Real >::Identity();
173                for( int i=0 ; i<3 ; i++ ) scale(i,i) = (Real)(1./_scale ) , trans(3,i) = -_center[i];
174                xForm = scale * trans * xForm;
175        }
176
177        {
178                double t = Time();
179                cnt = 0;
180                pointStream->reset();
181                Point3D< Real > p , n;
182                OrientedPoint3D< PointReal > _p;
183                while( pointStream->nextPoint( _p ) )
184                {
185                        p = xForm * Point3D< Real >(_p.p) , n = xFormN * Point3D< Real >(_p.n);
186                        if( !_InBounds(p) ) continue;
187                        Point3D< Real > myCenter = Point3D< Real >( Real(0.5) , Real(0.5) , Real(0.5) );
188                        Real myWidth = Real(1.0);
189                        Real weight=Real( 1. );
190                        if( useConfidence ) weight = Real( Length(n) );
191                        if( samplesPerNode>0 ) weight /= (Real)samplesPerNode;
192                        TreeOctNode* temp = _spaceRoot;
193                        int d=0;
194                        while( d<splatDepth )
195                        {
196                                _AddWeightContribution( densityWeights , temp , p , weightKey , weight );
197                                if( !temp->children ) temp->initChildren();
198                                int cIndex=TreeOctNode::CornerIndex( myCenter , p );
199                                temp = temp->children + cIndex;
200                                myWidth/=2;
201                                if( cIndex&1 ) myCenter[0] += myWidth/2;
202                                else           myCenter[0] -= myWidth/2;
203                                if( cIndex&2 ) myCenter[1] += myWidth/2;
204                                else           myCenter[1] -= myWidth/2;
205                                if( cIndex&4 ) myCenter[2] += myWidth/2;
206                                else           myCenter[2] -= myWidth/2;
207                                d++;
208                        }
209                        _AddWeightContribution( densityWeights , temp , p , weightKey , weight );
210                        cnt++;
211                }
212        }
213
214        std::vector< PointData< Real > >& points = pointInfo.data;
215
216        cnt = 0;
217        pointStream->reset();
218        Point3D< Real > p , n;
219        OrientedPoint3D< PointReal > _p;
220        Data _d;
221        while( ( dataValues ? pointStreamWithData->nextPoint( _p , _d ) : pointStream->nextPoint( _p ) ) )
222        {
223                p = xForm * Point3D< Real >(_p.p) , n = xFormN * Point3D< Real >(_p.n);
224                n *= Real(-1.);
225                if( !_InBounds(p) ) continue;
226                Real normalLength = Real( Length( n ) );
227		// [Bruno Levy] using Geogram's is_nan function (to overcome portability problems)
228                if( GEO::Numeric::is_nan( normalLength ) || normalLength<=EPSILON ) continue;
229                if( !useConfidence ) n /= normalLength;
230
231                Real pointWeight = Real(1.f);
232                if( samplesPerNode>0 )
233                {
234                        if( dataValues ) _MultiSplatPointData( &densityWeights , p , ProjectiveData< _Data >( _Data( _d ) , (Real)1. ) , *dataValues , weightKey , dataKey , maxDepth-1 , 2 );
235                        pointWeight = _SplatPointData( densityWeights , p , n , normalInfo , weightKey , normalKey , _minDepth-1 , maxDepth-1 , 3 );
236                }
237                else
238                {
239                        if( dataValues ) _MultiSplatPointData( ( SparseNodeData< Real , WeightDegree >* )NULL , p , ProjectiveData< _Data >( _Data( _d ) , (Real)1. ) , *dataValues , weightKey , dataKey , maxDepth-1 , 2 );
240
241                        Point3D< Real > myCenter = Point3D< Real >( Real(0.5) , Real(0.5) , Real(0.5) );
242                        Real myWidth = Real(1.0);
243                        TreeOctNode* temp = _spaceRoot;
244                        int d=0;
245                        if( splatDepth )
246                        {
247                                while( d<splatDepth )
248                                {
249                                        int cIndex=TreeOctNode::CornerIndex(myCenter,p);
250                                        temp = &temp->children[cIndex];
251                                        myWidth /= 2;
252                                        if(cIndex&1) myCenter[0] += myWidth/2;
253                                        else             myCenter[0] -= myWidth/2;
254                                        if(cIndex&2) myCenter[1] += myWidth/2;
255                                        else             myCenter[1] -= myWidth/2;
256                                        if(cIndex&4) myCenter[2] += myWidth/2;
257                                        else             myCenter[2] -= myWidth/2;
258                                        d++;
259                                }
260                                pointWeight = (Real)1.0/_GetSamplesPerNode( densityWeights , temp , p , weightKey );
261                        }
262                        for( i=0 ; i<DIMENSION ; i++ ) n[i] *= pointWeight;
263                        // [WARNING] mixing depth definitions
264                        while( d<maxDepth-1 )
265                        {
266                                if( !temp->children ) temp->initChildren();
267                                int cIndex=TreeOctNode::CornerIndex( myCenter , p );
268                                temp=&temp->children[cIndex];
269                                myWidth/=2;
270                                if(cIndex&1) myCenter[0] += myWidth/2;
271                                else             myCenter[0] -= myWidth/2;
272                                if(cIndex&2) myCenter[1] += myWidth/2;
273                                else             myCenter[1] -= myWidth/2;
274                                if(cIndex&4) myCenter[2] += myWidth/2;
275                                else             myCenter[2] -= myWidth/2;
276                                d++;
277                        }
278                        _SplatPointData( temp , p , n , normalInfo , normalKey );
279                }
280                pointWeightSum += pointWeight;
281                if( _constrainValues )
282                {
283                        Real pointScreeningWeight = useNormalWeights ? Real( normalLength ) : Real(1.f);
284                        TreeOctNode* temp = _spaceRoot;
285                        Point3D< Real > myCenter = Point3D< Real >( Real(0.5) , Real(0.5) , Real(0.5) );
286                        Real myWidth = Real(1.0);
287                        while( 1 )
288                        {
289                                if( (int)pointInfo.indices.size()<TreeNodeData::NodeCount ) pointInfo.indices.resize( TreeNodeData::NodeCount , -1 );
290                                int idx = pointInfo.index( temp );
291
292                                if( idx==-1 )
293                                {
294                                        idx = (int)points.size();
295                                        points.push_back( PointData< Real >( p*pointScreeningWeight , pointScreeningWeight ) );
296                                        pointInfo.indices[ temp->nodeData.nodeIndex ] = idx;
297                                }
298                                else
299                                {
300                                        points[idx].position += p*pointScreeningWeight;
301                                        points[idx].weight += pointScreeningWeight;
302                                }
303
304                                int cIndex = TreeOctNode::CornerIndex( myCenter , p );
305                                if( !temp->children ) break;
306                                temp = &temp->children[cIndex];
307                                myWidth /= 2;
308                                if( cIndex&1 ) myCenter[0] += myWidth/2;
309                                else               myCenter[0] -= myWidth/2;
310                                if( cIndex&2 ) myCenter[1] += myWidth/2;
311                                else               myCenter[1] -= myWidth/2;
312                                if( cIndex&4 ) myCenter[2] += myWidth/2;
313                                else               myCenter[2] -= myWidth/2;
314                        }
315                }
316                cnt++;
317        }
318
319        constraintWeight *= Real( pointWeightSum );
320        constraintWeight /= cnt;
321
322        MemoryUsage( );
323        if( _constrainValues )
324                // Set the average position and scale the weights
325                for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode(node) )
326                        if( pointInfo.index( node )!=-1 )
327                        {
328                                int idx = pointInfo.index( node );
329                                points[idx].position /= points[idx].weight;
330                                int e = _Depth( node ) * adaptiveExponent - ( maxDepth - 1 ) * (adaptiveExponent-1);
331                                if( e<0 ) points[idx].weight /= Real( 1<<(-e) );
332                                else      points[idx].weight *= Real( 1<<  e  );
333                                points[idx].weight *= Real( constraintWeight );
334                        }
335#if FORCE_NEUMANN_FIELD
336// #pragma message( "[WARNING] This is likely wrong as it only forces the normal component of the coefficient to be zero, not the actual vector-field" )
337        if( !_dirichlet )
338                for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode( node ) )
339                {
340                        int d , off[3] , res;
341                        node->depthAndOffset( d , off );
342                        res = 1<<d;
343                        int idx = normalInfo.index( node );
344                        if( idx<0 ) continue;
345                        Point3D< Real >& normal = normalInfo.data[ idx ];
346                        for( int d=0 ; d<3 ; d++ ) if( off[d]==0 || off[d]==res-1 ) normal[d] = 0;
347                }
348#endif // FORCE_NEUMANN_FIELD
349        nodeWeights.resize( TreeNodeData::NodeCount );
350        // Set the point weights for evaluating the iso-value
351        for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode(node) )
352        {
353                int nIdx = normalInfo.index( node );
354                if( nIdx>=0 )
355                {
356                        Real l = Real( Length( normalInfo.data[ nIdx ] ) );
357                        if( l )
358                        {
359                                int nIdx = nodeWeights.index( node );
360                                if( nIdx<0 )
361                                {
362                                        nodeWeights.indices[ node->nodeData.nodeIndex ] = (int)nodeWeights.data.size();
363                                        nodeWeights.data.push_back( l );
364                                }
365                                else nodeWeights.data[ nIdx ] = l;
366                        }
367                }
368        }
369        MemoryUsage();
370        if( makeComplete ) _MakeComplete( );
371        else _ClipTree< NormalDegree >( normalInfo );
372        _maxDepth = _tree.maxDepth();
373        return cnt;
374}
375
376template< class Real >
377void Octree< Real >::_SetValidityFlags( void )
378{
379        for( int i=0 ; i<_sNodes.end( _sNodes.levels()-1 ) ; i++ )
380        {
381                _sNodes.treeNodes[i]->nodeData.flags = 0;
382                if( IsValidNode< 0 >( _sNodes.treeNodes[i] , _dirichlet ) ) _sNodes.treeNodes[i]->nodeData.flags |= (1<<0);
383                if( IsValidNode< 1 >( _sNodes.treeNodes[i] , _dirichlet ) ) _sNodes.treeNodes[i]->nodeData.flags |= (1<<1);
384        }
385}
386template< class Real > void Octree< Real >::_MakeComplete( void ){ _tree.setFullDepth( _spaceRoot->maxDepth() ) ; MemoryUsage(); }
387// Trim off the branches of the tree (finer than _fullDepth) that don't contain normal points
388template< class Real >
389template< int NormalDegree >
390void Octree< Real >::_ClipTree( const SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo )
391{
392#if NEW_NEW_CODE
393#define ABS_INDEX( idx ) ( (idx<0) ? -(idx) : (idx) )
394        static const int      SupportSize   =  BSplineEvaluationData< NormalDegree >::SupportSize;
395        static const int  LeftSupportRadius = -BSplineEvaluationData< NormalDegree >::SupportStart;
396        static const int RightSupportRadius =  BSplineEvaluationData< NormalDegree >::SupportEnd;
397        int maxDepth = _tree.maxDepth();
398        typename TreeOctNode::NeighborKey< LeftSupportRadius , RightSupportRadius > neighborKey;
399        neighborKey.set( maxDepth );
400
401        // Set all nodes to invalid (negative indices)
402        for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode(node) ) node->nodeData.nodeIndex = -node->nodeData.nodeIndex;
403
404        // Iterate over the nodes and, if they contain a normal, make sure that the supported nodes exist and are set to valid
405        for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode(node) ) if( normalInfo.index( ABS_INDEX( node->nodeData.nodeIndex ) )>=0 )
406        {
407                int depth = node->depth();
408                neighborKey.getNeighbors< true >( node );
409                for( int d=0 ; d<=depth ; d++ )
410                {
411                        TreeOctNode::template Neighbors< SupportSize >& neighbors = neighborKey.neighbors[d];
412                        for( int i=0 ; i<SupportSize ; i++ ) for( int j=0 ; j<SupportSize ; j++ ) for( int k=0 ; k<SupportSize ; k++ )
413                                if( neighbors.neighbors[i][j][k] ) neighbors.neighbors[i][j][k]->nodeData.nodeIndex = ABS_INDEX( neighbors.neighbors[i][j][k]->nodeData.nodeIndex );
414                }
415        }
416
417        // Remove the invalid nodes
418        for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode( node ) )
419        {
420                if( node->children && _Depth(node)>=_fullDepth )
421                {
422                        bool hasValidChildren = false;
423                        for( int c=0 ; c<Cube::CORNERS ; c++ ) hasValidChildren |= ( node->children[c].nodeData.nodeIndex>0 );
424                        if( !hasValidChildren ) node->children = NULL;
425                }
426                node->nodeData.nodeIndex = ABS_INDEX( node->nodeData.nodeIndex );
427        }
428
429        MemoryUsage();
430#undef ABS_INDEX
431#else // !NEW_NEW_CODE
432        int maxDepth = _tree.maxDepth();
433        for( TreeOctNode* temp=_tree.nextNode() ; temp ; temp=_tree.nextNode(temp) )
434                if( temp->children && _Depth( temp )>=_fullDepth )
435                {
436                        int hasNormals=0;
437                        for( int i=0 ; i<Cube::CORNERS && !hasNormals ; i++ ) hasNormals = _HasNormals( &temp->children[i] , normalInfo );
438                        if( !hasNormals ) temp->children=NULL;
439                }
440        MemoryUsage();
441#endif // NEW_NEW_CODE
442}
443
444template< class Real >
445template< int FEMDegree >
446void Octree< Real >::EnableMultigrid( std::vector< int >* map )
447{
448        if( FEMDegree<=_multigridDegree ) return;
449        _multigridDegree = FEMDegree;
450        const int OverlapRadius = -BSplineIntegrationData< FEMDegree , FEMDegree >::OverlapStart;
451        int maxDepth = _tree.maxDepth( );
452        typename TreeOctNode::NeighborKey< OverlapRadius , OverlapRadius > neighborKey;
453        neighborKey.set( maxDepth-1 );
454        for( int d=maxDepth-1 ; d>=0 ; d-- )
455                for( TreeOctNode* node=_tree.nextNode() ; node ; node=_tree.nextNode( node ) ) if( node->depth()==d && node->children )
456                        neighborKey.template getNeighbors< true >( node );
457        _sNodes.set( _tree , map );
458        _SetValidityFlags();
459}
460
461template< class Real >
462template< int NormalDegree >
463int Octree< Real >::_HasNormals( TreeOctNode* node , const SparseNodeData< Point3D< Real > , NormalDegree >& normalInfo )
464{
465        int idx = normalInfo.index( node );
466        if( idx>=0 )
467        {
468                const Point3D< Real >& normal = normalInfo.data[ idx ];
469                if( normal[0]!=0 || normal[1]!=0 || normal[2]!=0 ) return 1;
470        }
471        if( node->children ) for( int i=0 ; i<Cube::CORNERS ; i++ ) if( _HasNormals( &node->children[i] , normalInfo ) ) return 1;
472        return 0;
473}
474
475////////////////
476// VertexData //
477////////////////
478long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth)
479{
480        int idx[DIMENSION];
481        return CenterIndex(node,maxDepth,idx);
482}
483long long VertexData::CenterIndex(const TreeOctNode* node,int maxDepth,int idx[DIMENSION])
484{
485        int d,o[3];
486        node->depthAndOffset(d,o);
487        for(int i=0;i<DIMENSION;i++) idx[i]=BinaryNode::CornerIndex( maxDepth+1 , d+1 , o[i]<<1 , 1 );
488        return (long long)(idx[0]) | (long long)(idx[1])<<VERTEX_COORDINATE_SHIFT | (long long)(idx[2])<<(2*VERTEX_COORDINATE_SHIFT);
489}
490long long VertexData::CenterIndex( int depth , const int offSet[DIMENSION] , int maxDepth , int idx[DIMENSION] )
491{
492        for(int i=0;i<DIMENSION;i++) idx[i]=BinaryNode::CornerIndex( maxDepth+1 , depth+1 , offSet[i]<<1 , 1 );
493        return (long long)(idx[0]) | (long long)(idx[1])<<VERTEX_COORDINATE_SHIFT | (long long)(idx[2])<<(2*VERTEX_COORDINATE_SHIFT);
494}
495long long VertexData::CornerIndex(const TreeOctNode* node,int cIndex,int maxDepth)
496{
497        int idx[DIMENSION];
498        return CornerIndex(node,cIndex,maxDepth,idx);
499}
500long long VertexData::CornerIndex( const TreeOctNode* node , int cIndex , int maxDepth , int idx[DIMENSION] )
501{
502        int x[DIMENSION];
503        Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] );
504        int d , o[3];
505        node->depthAndOffset( d , o );
506        for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode::CornerIndex( maxDepth+1 , d , o[i] , x[i] );
507        return CornerIndexKey( idx );
508}
509long long VertexData::CornerIndex( int depth , const int offSet[DIMENSION] , int cIndex , int maxDepth , int idx[DIMENSION] )
510{
511        int x[DIMENSION];
512        Cube::FactorCornerIndex( cIndex , x[0] , x[1] , x[2] );
513        for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode::CornerIndex( maxDepth+1 , depth , offSet[i] , x[i] );
514        return CornerIndexKey( idx );
515}
516long long VertexData::CornerIndexKey( const int idx[DIMENSION] )
517{
518        return (long long)(idx[0]) | (long long)(idx[1])<<VERTEX_COORDINATE_SHIFT | (long long)(idx[2])<<(2*VERTEX_COORDINATE_SHIFT);
519}
520long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth){
521        int idx[DIMENSION];
522        return FaceIndex(node,fIndex,maxDepth,idx);
523}
524long long VertexData::FaceIndex(const TreeOctNode* node,int fIndex,int maxDepth,int idx[DIMENSION])
525{
526        int dir,offset;
527        Cube::FactorFaceIndex(fIndex,dir,offset);
528        int d,o[3];
529        node->depthAndOffset(d,o);
530        for(int i=0;i<DIMENSION;i++){idx[i]=BinaryNode::CornerIndex(maxDepth+1,d+1,o[i]<<1,1);}
531        idx[dir]=BinaryNode::CornerIndex(maxDepth+1,d,o[dir],offset);
532        return (long long)(idx[0]) | (long long)(idx[1])<<VERTEX_COORDINATE_SHIFT | (long long)(idx[2])<<(2*VERTEX_COORDINATE_SHIFT);
533}
534long long VertexData::EdgeIndex( const TreeOctNode* node , int eIndex , int maxDepth ){ int idx[DIMENSION] ; return EdgeIndex( node , eIndex , maxDepth , idx ); }
535long long VertexData::EdgeIndex( const TreeOctNode* node , int eIndex , int maxDepth , int idx[DIMENSION] )
536{
537        int o , i1 , i2;
538        int d , off[3];
539        node->depthAndOffset( d ,off );
540        Cube::FactorEdgeIndex( eIndex , o , i1 , i2 );
541        for( int i=0 ; i<DIMENSION ; i++ ) idx[i] = BinaryNode::CornerIndex( maxDepth+1 , d+1 , off[i]<<1 , 1 );
542        switch(o)
543        {
544                case 0:
545                        idx[1] = BinaryNode::CornerIndex( maxDepth+1 , d , off[1] , i1 );
546                        idx[2] = BinaryNode::CornerIndex( maxDepth+1 , d , off[2] , i2 );
547                        break;
548                case 1:
549                        idx[0] = BinaryNode::CornerIndex( maxDepth+1 , d , off[0] , i1 );
550                        idx[2] = BinaryNode::CornerIndex( maxDepth+1 , d , off[2] , i2 );
551                        break;
552                case 2:
553                        idx[0] = BinaryNode::CornerIndex( maxDepth+1 , d , off[0] , i1 );
554                        idx[1] = BinaryNode::CornerIndex( maxDepth+1 , d , off[1] , i2 );
555                        break;
556        };
557        return (long long)(idx[0]) | (long long)(idx[1])<<VERTEX_COORDINATE_SHIFT | (long long)(idx[2])<<(2*VERTEX_COORDINATE_SHIFT);
558}
559