/*
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 "Octree.h"

#define ITERATION_POWER 1.0/3
#define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12

#define READ_SIZE 1024

#define PAD_SIZE (Real(1.0))

const Real EPSILON=Real(1e-6);
const Real ROUND_EPS=Real(1e-5);


/////////////////////
// SortedTreeNodes //
/////////////////////
SortedTreeNodes::SortedTreeNodes(void){
	nodeCount=NULL;
	treeNodes=NULL;
	maxDepth=0;
}
SortedTreeNodes::~SortedTreeNodes(void){
	if(nodeCount){delete[] nodeCount;}
	if(treeNodes){delete[] treeNodes;}
	nodeCount=NULL;
	treeNodes=NULL;
}
void SortedTreeNodes::set(TreeOctNode& root,const int& setIndex){
	if(nodeCount){delete[] nodeCount;}
	if(treeNodes){delete[] treeNodes;}
	maxDepth=root.maxDepth()+1;
	nodeCount=new int[maxDepth+1];
	treeNodes=new TreeOctNode*[root.nodes()];

	TreeOctNode* temp=root.nextNode();
	int i,cnt=0;
	while(temp){
		treeNodes[cnt++]=temp;
		temp=root.nextNode(temp);
	}
	qsort(treeNodes,cnt,sizeof(const TreeOctNode*),TreeOctNode::CompareForwardPointerDepths);
	for(i=0;i<=maxDepth;i++){nodeCount[i]=0;}
	for(i=0;i<cnt;i++){
		if(setIndex){treeNodes[i]->nodeData.nodeIndex=i;}
		nodeCount[treeNodes[i]->depth()+1]++;
	}
	for(i=1;i<=maxDepth;i++){nodeCount[i]+=nodeCount[i-1];}
}


//////////////////
// TreeNodeData //
//////////////////
int TreeNodeData::UseIndex=1;
TreeNodeData::TreeNodeData(void){
	if(UseIndex){
		nodeIndex=-1;
		centerWeightContribution=0;
	}
	else{mcIndex=0;}
	value=0;
}
TreeNodeData::~TreeNodeData(void){;}


////////////
// Octree //
////////////
template<int Degree>
double Octree<Degree>::maxMemoryUsage=0;

//template<int Degree>
//double Octree<Degree>::MemoryUsage(void){
//	double mem=MemoryInfo::Usage()/(1<<20);
//	if(mem>maxMemoryUsage){maxMemoryUsage=mem;}
//	return mem;
//}

template<int Degree>
Octree<Degree>::Octree(void){
	radius=0;
	width=0;
	postNormalSmooth=0;
}

template<int Degree>
void Octree<Degree>::setNodeIndices(TreeOctNode& node,int& idx){
	node.nodeData.nodeIndex=idx;
	idx++;
	if(node.children){for(int i=0;i<Cube::CORNERS;i++){setNodeIndices(node.children[i],idx);}}
}
template<int Degree>
int Octree<Degree>::NonLinearSplatOrientedPoint(TreeOctNode* node,const Point3D<Real>& position,const Point3D<Real>& normal){
	double x,dxdy,dxdydz,dx[DIMENSION][3];
	int i,j,k;
	TreeOctNode::Neighbors& neighbors=neighborKey.setNeighbors(node);
	double width;
	Point3D<Real> center;
	Real w;

	node->centerAndWidth(center,w);
	width=w;
	for(int i=0;i<3;i++){
		x=(center.coords[i]-position.coords[i]-width)/width;
		dx[i][0]=1.125+1.500*x+0.500*x*x;
		x=(center.coords[i]-position.coords[i])/width;
		dx[i][1]=0.750        -      x*x;
		dx[i][2]=1.0-dx[i][1]-dx[i][0];
	}
	for(i=0;i<3;i++){
		for(j=0;j<3;j++){
			dxdy=dx[0][i]*dx[1][j];
			for(k=0;k<3;k++){
				if(neighbors.neighbors[i][j][k]){
					dxdydz=dxdy*dx[2][k];
					int idx=neighbors.neighbors[i][j][k]->nodeData.nodeIndex;
					if(idx<0){
						Point3D<Real> n;
						n.coords[0]=n.coords[1]=n.coords[2]=0;
						idx=neighbors.neighbors[i][j][k]->nodeData.nodeIndex=int(normals->size());
						normals->push_back(n);
					}
					(*normals)[idx].coords[0]+=Real(normal.coords[0]*dxdydz);
					(*normals)[idx].coords[1]+=Real(normal.coords[1]*dxdydz);
					(*normals)[idx].coords[2]+=Real(normal.coords[2]*dxdydz);
				}
			}
		}
	}
	return 0;
}
template<int Degree>
void Octree<Degree>::NonLinearSplatOrientedPoint(const Point3D<Real>& position,const Point3D<Real>& normal,const int& splatDepth,const Real& samplesPerNode,
												 const int& minDepth,const int& maxDepth){
	double dx;
	Point3D<Real> n;
	TreeOctNode* temp;
	int i,cnt=0;
	double width;
	Point3D<Real> myCenter;
	Real myWidth;
	myCenter.coords[0]=myCenter.coords[1]=myCenter.coords[2]=Real(0.5);
	myWidth=Real(1.0);

	temp=&tree;
	while(temp->depth()<splatDepth){
		if(!temp->children){
			printf("Octree<Degree>::NonLinearSplatOrientedPoint error\n");
			return;
		}
		int cIndex=TreeOctNode::CornerIndex(myCenter,position);
		temp=&temp->children[cIndex];
		myWidth/=2;
		if(cIndex&1){myCenter.coords[0]+=myWidth/2;}
		else		{myCenter.coords[0]-=myWidth/2;}
		if(cIndex&2){myCenter.coords[1]+=myWidth/2;}
		else		{myCenter.coords[1]-=myWidth/2;}
		if(cIndex&4){myCenter.coords[2]+=myWidth/2;}
		else		{myCenter.coords[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.coords[0]+=myWidth/2;}
		else		{myCenter.coords[0]-=myWidth/2;}
		if(cIndex&2){myCenter.coords[1]+=myWidth/2;}
		else		{myCenter.coords[1]-=myWidth/2;}
		if(cIndex&4){myCenter.coords[2]+=myWidth/2;}
		else		{myCenter.coords[2]-=myWidth/2;}
	}
	width=1.0/(1<<temp->depth());
	for(i=0;i<DIMENSION;i++){n.coords[i]=normal.coords[i]*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());

		for(i=0;i<DIMENSION;i++){n.coords[i]=normal.coords[i]*alpha/Real(pow(width,3))*Real(dx);}
		NonLinearSplatOrientedPoint(temp,position,n);
	}
}
template<int Degree>
void Octree<Degree>::NonLinearGetSampleDepthAndWeight(TreeOctNode* node,const Point3D<Real>& position,const Real& samplesPerNode,Real& depth,Real& weight){
	TreeOctNode* temp=node;
	weight=Real(1.0)/NonLinearGetSampleWeight(temp,position);
	if(weight>=samplesPerNode+1){depth=Real(temp->depth()+log(weight/(samplesPerNode+1))/log(double(1<<(DIMENSION-1))));}
	else{
		Real oldAlpha,newAlpha;
		oldAlpha=newAlpha=weight;
		while(newAlpha<(samplesPerNode+1) && temp->parent){
			temp=temp->parent;
			oldAlpha=newAlpha;
			newAlpha=Real(1.0)/NonLinearGetSampleWeight(temp,position);
		}
		depth=Real(temp->depth()+log(newAlpha/(samplesPerNode+1))/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];
	int i,j,k;
	TreeOctNode::Neighbors& neighbors=neighborKey.setNeighbors(node);
	double width;
	Point3D<Real> center;
	Real w;
	node->centerAndWidth(center,w);
	width=w;

	for(i=0;i<DIMENSION;i++){
		x=(center.coords[i]-position.coords[i]-width)/width;
		dx[i][0]=1.125+1.500*x+0.500*x*x;
		x=(center.coords[i]-position.coords[i])/width;
		dx[i][1]=0.750        -      x*x;
		dx[i][2]=1.0-dx[i][1]-dx[i][0];
	}

	for(i=0;i<3;i++){
		for(j=0;j<3;j++){
			dxdy=dx[0][i]*dx[1][j];
			for(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,const Real& weight){
	int i,j,k;
	TreeOctNode::Neighbors& neighbors=neighborKey.setNeighbors(node);
	double x,dxdy,dx[DIMENSION][3];
	double width;
	Point3D<Real> center;
	Real w;
	node->centerAndWidth(center,w);
	width=w;

	for(i=0;i<DIMENSION;i++){
		x=(center.coords[i]-position.coords[i]-width)/width;
		dx[i][0]=1.125+1.500*x+0.500*x*x;
		x=(center.coords[i]-position.coords[i])/width;
		dx[i][1]=0.750        -      x*x;
		dx[i][2]=1.0-dx[i][1]-dx[i][0];
	}

	for(i=0;i<3;i++){
		for(j=0;j<3;j++){
			dxdy=dx[0][i]*dx[1][j]*weight;
			for(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>
int Octree<Degree>::setTree(std::vector<Point3D<Real> > &Pts, std::vector<Point3D<Real> > &Nor,
							const int& maxDepth,
							const int& kernelDepth,const Real& samplesPerNode,const Real& scaleFactor,Point3D<Real>& center,Real& scale,
							const int& resetSamples,const int& useConfidence){

	Point3D<Real> min,max,position,normal,myCenter;
	Real myWidth;
	TreeOctNode* temp;
	int splatDepth=0;
	float c[2*DIMENSION];

	TreeNodeData::UseIndex=1;
	neighborKey.set(maxDepth);
	splatDepth=kernelDepth;
	if(splatDepth<0){splatDepth=0;}

	//DumpOutput("Setting bounding box\n");
	// Read through once to get the center and scale
	int i,cnt=0;
	for(int pi=0;pi<Pts.size();++pi)
	{
		//if(binary){if(fread(c,sizeof(float),2*DIMENSION,fp)!=6){break;}}
		//else{if(fscanf(fp," %f %f %f %f %f %f ",&c[0],&c[1],&c[2],&c[3],&c[4],&c[5])!=2*DIMENSION){break;}}
		for(i=0;i<DIMENSION;i++){
		  c[i]=Pts[pi].coords[i];
			if(!cnt || c[i]<min.coords[i]){min.coords[i]=c[i];}
			if(!cnt || c[i]>max.coords[i]){max.coords[i]=c[i];}
		}
		cnt++;
	}
	for(i=0;i<DIMENSION;i++){
		if(!i || scale<max.coords[i]-min.coords[i]){scale=Real(max.coords[i]-min.coords[i]);}
		center.coords[i]=Real(max.coords[i]+min.coords[i])/2;
	}
	printf("Samples: %d\n",cnt);
	scale*=scaleFactor;
	for(i=0;i<DIMENSION;i++){center.coords[i]-=scale/2;}
	if(splatDepth>0){
		printf("Setting sample weights\n");
		cnt=0;
		//fseek(fp,SEEK_SET,0);
			for(int pi=0;pi<Pts.size();++pi)
			{
			//if(binary){if(fread(c,sizeof(float),2*DIMENSION,fp)!=2*DIMENSION){break;}}
			//else{if(fscanf(fp," %f %f %f %f %f %f ",&c[0],&c[1],&c[2],&c[3],&c[4],&c[5])!=2*DIMENSION){break;}}
			for(i=0;i<DIMENSION;i++){
				c[i]=Pts[pi].coords[i];
				c[i+3]=Nor[pi].coords[i];
				position.coords[i]=(c[i]-center.coords[i])/scale;
				normal.coords[i]=c[DIMENSION+i];
			}
			myCenter.coords[0]=myCenter.coords[1]=myCenter.coords[2]=Real(0.5);
			myWidth=Real(1.0);
			for(i=0;i<DIMENSION;i++){if(position.coords[i]<myCenter.coords[i]-myWidth/2 || position.coords[i]>myCenter.coords[i]+myWidth/2){break;}}
			if(i!=DIMENSION){continue;}
			temp=&tree;
			int d=0;
			Real weight=Real(1.0);
			if(useConfidence){weight=Real(Length(normal));}
			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.coords[0]+=myWidth/2;}
				else		{myCenter.coords[0]-=myWidth/2;}
				if(cIndex&2){myCenter.coords[1]+=myWidth/2;}
				else		{myCenter.coords[1]-=myWidth/2;}
				if(cIndex&4){myCenter.coords[2]+=myWidth/2;}
				else		{myCenter.coords[2]-=myWidth/2;}
				d++;
			}
			NonLinearUpdateWeightContribution(temp,position,weight);
			cnt++;
		}
	}

	printf("Adding Points and Normals\n");
	normals=new std::vector<Point3D<Real> >();
	cnt=0;
	//fseek(fp,SEEK_SET,0);
	for(int pi=0;pi<Pts.size();++pi)
		{
		//if(binary){if(fread(c,sizeof(float),2*DIMENSION,fp)!=2*DIMENSION){break;}}
		//else{if(fscanf(fp," %f %f %f %f %f %f ",&c[0],&c[1],&c[2],&c[3],&c[4],&c[5])!=2*DIMENSION){break;}}
		for(i=0;i<DIMENSION;i++){
			c[i]=Pts[pi].coords[i];
			c[i+3]=Nor[pi].coords[i];
			position.coords[i]=(c[i]-center.coords[i])/scale;
			normal.coords[i]=c[DIMENSION+i];
		}
		myCenter.coords[0]=myCenter.coords[1]=myCenter.coords[2]=Real(0.5);
		myWidth=Real(1.0);
		for(i=0;i<DIMENSION;i++){if(position.coords[i]<myCenter.coords[i]-myWidth/2 || position.coords[i]>myCenter.coords[i]+myWidth/2){break;}}
		if(i!=DIMENSION){continue;}
		Real l=Real(Length(normal));
		if(l<EPSILON){continue;}
		if(!useConfidence){
			normal.coords[0]/=l;
			normal.coords[1]/=l;
			normal.coords[2]/=l;
		}
		l=Real(2<<maxDepth);
		normal.coords[0]*=l;
		normal.coords[1]*=l;
		normal.coords[2]*=l;

		if(resetSamples && samplesPerNode>0 && splatDepth){
			NonLinearSplatOrientedPoint(position,normal,splatDepth,samplesPerNode,1,maxDepth);
		}
		else{
			Real alpha=1;
			temp=&tree;
			if(splatDepth){
				int d=0;
				while(d<splatDepth){
					int cIndex=TreeOctNode::CornerIndex(myCenter,position);
					temp=&temp->children[cIndex];
					myWidth/=2;
					if(cIndex&1){myCenter.coords[0]+=myWidth/2;}
					else		{myCenter.coords[0]-=myWidth/2;}
					if(cIndex&2){myCenter.coords[1]+=myWidth/2;}
					else		{myCenter.coords[1]-=myWidth/2;}
					if(cIndex&4){myCenter.coords[2]+=myWidth/2;}
					else		{myCenter.coords[2]-=myWidth/2;}
					d++;
				}
				alpha=NonLinearGetSampleWeight(temp,position);
			}
			for(i=0;i<DIMENSION;i++){normal.coords[i]*=alpha;}
			int d=0;
			while(d<maxDepth){
				if(!temp->children){temp->initChildren();}
				int cIndex=TreeOctNode::CornerIndex(myCenter,position);
				temp=&temp->children[cIndex];
				myWidth/=2;
				if(cIndex&1){myCenter.coords[0]+=myWidth/2;}
				else		{myCenter.coords[0]-=myWidth/2;}
				if(cIndex&2){myCenter.coords[1]+=myWidth/2;}
				else		{myCenter.coords[1]-=myWidth/2;}
				if(cIndex&4){myCenter.coords[2]+=myWidth/2;}
				else		{myCenter.coords[2]-=myWidth/2;}
				d++;
			}
			NonLinearSplatOrientedPoint(temp,position,normal);
		}
	}
//	DumpOutput("Memory Usage: %.3f MB\n",float(MemoryUsage()));
//	fclose(fp);
	return cnt;
}

template<int Degree>
void Octree<Degree>::setFunctionData(const PPolynomial<Degree>& ReconstructionFunction,	const int& maxDepth,const int& normalize,const Real& normalSmooth){

	radius=Real(fabs(ReconstructionFunction.polys[0].start));
	width=int(double(radius+0.5-EPSILON)*2);
	if(normalSmooth>0){postNormalSmooth=normalSmooth;}
	fData.set(maxDepth,ReconstructionFunction,normalize,1);
}

template<int Degree>
void Octree<Degree>::finalize1(const int& refineNeighbors)
{
	TreeOctNode* temp;

	if(refineNeighbors>=0){
		RefineFunction rf;
		temp=tree.nextNode();
		while(temp){
			if(temp->nodeData.nodeIndex>=0 && Length((*normals)[temp->nodeData.nodeIndex])>EPSILON){
				rf.depth=temp->depth()-refineNeighbors;
				TreeOctNode::ProcessMaxDepthNodeAdjacentNodes(fData.depth,temp,2*width,&tree,1,temp->depth()-refineNeighbors,&rf);
			}
			temp=tree.nextNode(temp);
		}
	}
	else if(refineNeighbors==-1234){
		temp=tree.nextLeaf();
		while(temp){
			if(!temp->children && temp->depth()<fData.depth){temp->initChildren();}
			temp=tree.nextLeaf(temp);
		}
	}
}
template<int Degree>
void Octree<Degree>::finalize2(const int& refineNeighbors)
{
	TreeOctNode* temp;

	if(refineNeighbors>=0){
		RefineFunction rf;
		temp=tree.nextNode();
		while(temp){
			if(fabs(temp->nodeData.value)>EPSILON){
				rf.depth=temp->depth()-refineNeighbors;
				TreeOctNode::ProcessMaxDepthNodeAdjacentNodes(fData.depth,temp,2*width,&tree,1,temp->depth()-refineNeighbors,&rf);
			}
			temp=tree.nextNode(temp);
		}
	}
}
template <int Degree>
Real Octree<Degree>::GetDivergence(const int idx[DIMENSION],const Point3D<Real>& normal) const
{
	double dot=fData.dotTable[idx[0]]*fData.dotTable[idx[1]]*fData.dotTable[idx[2]];
	return Real(dot*(fData.dDotTable[idx[0]]*normal.coords[0]+fData.dDotTable[idx[1]]*normal.coords[1]+fData.dDotTable[idx[2]]*normal.coords[2]));
}
template<int Degree>
Real Octree<Degree>::GetLaplacian(const int idx[DIMENSION]) const
{
	return Real(fData.dotTable[idx[0]]*fData.dotTable[idx[1]]*fData.dotTable[idx[2]]*(fData.d2DotTable[idx[0]]+fData.d2DotTable[idx[1]]+fData.d2DotTable[idx[2]]));
}
template<int Degree>
Real Octree<Degree>::GetDotProduct(const int idx[DIMENSION]) const
{
	return Real(fData.dotTable[idx[0]]*fData.dotTable[idx[1]]*fData.dotTable[idx[2]]);
}

template<int Degree>
int Octree<Degree>::GetFixedDepthLaplacian(SparseSymmetricMatrix<float>& matrix,const int& depth,const SortedTreeNodes& sNodes)
{
	LaplacianMatrixFunction mf;
	mf.ot=this;
	mf.offset=sNodes.nodeCount[depth];
	matrix.Resize(sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth]);
	mf.rowElements=(MatrixEntry<float>*)malloc(sizeof(MatrixEntry<float>)*matrix.rows);
	for(int i=sNodes.nodeCount[depth];i<sNodes.nodeCount[depth+1];i++){
		mf.elementCount=0;
		mf.d2=int(sNodes.treeNodes[i]->d);
		mf.x2=int(sNodes.treeNodes[i]->off[0]);
		mf.y2=int(sNodes.treeNodes[i]->off[1]);
		mf.z2=int(sNodes.treeNodes[i]->off[2]);
		mf.index[0]=mf.x2;
		mf.index[1]=mf.y2;
		mf.index[2]=mf.z2;
		TreeOctNode::ProcessTerminatingNodeAdjacentNodes(fData.depth,sNodes.treeNodes[i],2*width-1,&tree,1,&mf);
		matrix.SetRowSize(i-sNodes.nodeCount[depth],mf.elementCount);
		memcpy(matrix.m_ppElements[i-sNodes.nodeCount[depth]],mf.rowElements,sizeof(MatrixEntry<float>)*mf.elementCount);
	}
	free(mf.rowElements);
	return 1;
}
template<int Degree>
int Octree<Degree>::GetRestrictedFixedDepthLaplacian(SparseSymmetricMatrix<float>& matrix,const int& depth,const int* entries,const int& entryCount,
													 const TreeOctNode* rNode,const Real& radius,
													 const SortedTreeNodes& sNodes){
	int i;
	RestrictedLaplacianMatrixFunction mf;
	Real myRadius=int(2*radius-ROUND_EPS)+ROUND_EPS;
	mf.ot=this;
	mf.radius=radius;
	rNode->depthAndOffset(mf.depth,mf.offset);
	matrix.Resize(entryCount);
	mf.rowElements=(MatrixEntry<float>*)malloc(sizeof(MatrixEntry<float>)*matrix.rows);
	for(i=0;i<entryCount;i++){sNodes.treeNodes[entries[i]]->nodeData.nodeIndex=i;}
	for(i=0;i<entryCount;i++){
		mf.elementCount=0;
		mf.index[0]=int(sNodes.treeNodes[entries[i]]->off[0]);
		mf.index[1]=int(sNodes.treeNodes[entries[i]]->off[1]);
		mf.index[2]=int(sNodes.treeNodes[entries[i]]->off[2]);
		TreeOctNode::ProcessTerminatingNodeAdjacentNodes(fData.depth,sNodes.treeNodes[entries[i]],2*width-1,&tree,1,&mf);
		matrix.SetRowSize(i,mf.elementCount);
		memcpy(matrix.m_ppElements[i],mf.rowElements,sizeof(MatrixEntry<float>)*mf.elementCount);
	}
	for(i=0;i<entryCount;i++){sNodes.treeNodes[entries[i]]->nodeData.nodeIndex=entries[i];}
	free(mf.rowElements);
	return 1;
}


template<int Degree>
int Octree<Degree>::LaplacianMatrixIteration(const int& subdivideDepth){
	int i,iter=0;
	SortedTreeNodes sNodes;
	double t;
	fData.setDotTables(fData.D2_DOT_FLAG);
	sNodes.set(tree,1);

	SparseMatrix<float>::SetAllocator(MEMORY_ALLOCATOR_BLOCK_SIZE);

	sNodes.treeNodes[0]->nodeData.value=0;
	for(i=1;i<sNodes.maxDepth;i++){
		printf("Depth: %d/%d\n",i,sNodes.maxDepth-1);
//		t=Time();
		if(subdivideDepth>0){iter+=SolveFixedDepthMatrix(i,subdivideDepth,sNodes);}
		else{iter+=SolveFixedDepthMatrix(i,sNodes);}
	}
	SparseMatrix<float>::Allocator.reset();
	fData.clearDotTables(fData.DOT_FLAG | fData.D_DOT_FLAG | fData.D2_DOT_FLAG);
	return iter;
}

template<int Degree>
int Octree<Degree>::SolveFixedDepthMatrix(const int& depth,const SortedTreeNodes& sNodes){
	int i,iter=0;
	Vector<double> V,Solution;
	SparseSymmetricMatrix<Real> matrix;
	Real myRadius;
	double gTime,sTime,uTime;
	Real dx,dy,dz;
	int x1,x2,y1,y2,z1,z2;
	Vector<Real> Diagonal;

//	gTime=Time();
	V.Resize(sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth]);
	for(i=sNodes.nodeCount[depth];i<sNodes.nodeCount[depth+1];i++){V[i-sNodes.nodeCount[depth]]=sNodes.treeNodes[i]->nodeData.value;}
	SparseSymmetricMatrix<float>::Allocator.rollBack();
	GetFixedDepthLaplacian(matrix,depth,sNodes);
//	gTime=Time()-gTime;
	printf("\tMatrix entries: %d / %d^2 = %.4f%%\n",matrix.Entries(),matrix.rows,100.0*(matrix.Entries()/double(matrix.rows))/matrix.rows);
//	DumpOutput("\tMemory Usage: %.3f MB\n",float(MemoryUsage()));
//	sTime=Time();
	iter+=SparseSymmetricMatrix<Real>::Solve(matrix,V,int(pow(matrix.rows,ITERATION_POWER)),Solution,double(EPSILON),1);
//	sTime=Time()-sTime;
//	uTime=Time();
	for(i=sNodes.nodeCount[depth];i<sNodes.nodeCount[depth+1];i++){sNodes.treeNodes[i]->nodeData.value=Real(Solution[i-sNodes.nodeCount[depth]]);}

	myRadius=Real(radius+ROUND_EPS-0.5);
	myRadius /=(1<<depth);

	if(depth<sNodes.maxDepth-1){
		LaplacianProjectionFunction pf;
		TreeOctNode *node1,*node2;
		pf.ot=this;
		int idx1,idx2,off=sNodes.nodeCount[depth];
		// First pass: idx2 is the solution coefficient propogated
		for(i=0;i<matrix.rows;i++){
			idx1=i;
			node1=sNodes.treeNodes[idx1+off];
			if(!node1->children){continue;}
			x1=int(node1->off[0]);
			y1=int(node1->off[1]);
			z1=int(node1->off[2]);
			for(int j=0;j<matrix.rowSizes[i];j++){
				idx2=matrix.m_ppElements[i][j].N;
				node2=sNodes.treeNodes[idx2+off];
				x2=int(node2->off[0]);
				y2=int(node2->off[1]);
				z2=int(node2->off[2]);
				pf.value=Solution[idx2];
				pf.index[0]=x2;
				pf.index[1]=y2;
				pf.index[2]=z2;
				dx=Real(x2-x1)/(1<<depth);
				dy=Real(y2-y1)/(1<<depth);
				dz=Real(z2-z1)/(1<<depth);
				if(fabs(dx)<myRadius && fabs(dy)<myRadius && fabs(dz)<myRadius){node1->processNodeNodes(node2,&pf,0);}
				else{TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,node2,width,node1,width,&pf,0);}
			}
		}
		// Second pass: idx1 is the solution coefficient propogated
		for(i=0;i<matrix.rows;i++){
			idx1=i;
			node1=sNodes.treeNodes[idx1+off];
			x1=int(node1->off[0]);
			y1=int(node1->off[1]);
			z1=int(node1->off[2]);
			pf.value=Solution[idx1];
			pf.index[0]=x1;
			pf.index[1]=y1;
			pf.index[2]=z1;
			for(int j=0;j<matrix.rowSizes[i];j++){
				idx2=matrix.m_ppElements[i][j].N;
				node2=sNodes.treeNodes[idx2+off];
				if(idx1!=idx2 && node2->children){
					x2=int(node2->off[0]);
					y2=int(node2->off[1]);
					z2=int(node2->off[2]);
					dx=Real(x1-x2)/(1<<depth);
					dy=Real(y1-y2)/(1<<depth);
					dz=Real(z1-z2)/(1<<depth);
					if(fabs(dx)<myRadius && fabs(dy)<myRadius && fabs(dz)<myRadius){node2->processNodeNodes(node1,&pf,0);}
					else{TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,node1,width,node2,width,&pf,0);}
				}
			}
		}
	}
//	uTime=Time()-uTime;
	printf("\tGot / Solved / Updated in: %6.3f / %6.3f / %6.3f\n",gTime,sTime,uTime);
	return iter;
}
template<int Degree>
int Octree<Degree>::SolveFixedDepthMatrix(const int& depth,const int& startingDepth,const SortedTreeNodes& sNodes){
	int i,j,d,iter=0;
	SparseSymmetricMatrix<Real> matrix;
	AdjacencySetFunction asf;
	AdjacencyCountFunction acf;
	Vector<Real> Values;
	Vector<double> SubValues,SubSolution;
	double gTime,sTime,uTime;
	Real myRadius,myRadius2;
	Real dx,dy,dz;
	Vector<Real> Diagonal;

	if(startingDepth>=depth){return SolveFixedDepthMatrix(depth,sNodes);}

	Values.Resize(sNodes.nodeCount[depth+1]-sNodes.nodeCount[depth]);

	for(i=sNodes.nodeCount[depth];i<sNodes.nodeCount[depth+1];i++){
		Values[i-sNodes.nodeCount[depth]]=sNodes.treeNodes[i]->nodeData.value;
		sNodes.treeNodes[i]->nodeData.value=0;
	}

	myRadius=2*radius-Real(0.5);
	myRadius=int(myRadius-ROUND_EPS)+ROUND_EPS;
	myRadius2=Real(radius+ROUND_EPS-0.5);
	d=depth-startingDepth;
	for(i=sNodes.nodeCount[d];i<sNodes.nodeCount[d+1];i++){
//		gTime=Time();
		TreeOctNode* temp;
		// Get all of the entries associated to the subspace
		acf.adjacencyCount=0;
		temp=sNodes.treeNodes[i]->nextNode();
		while(temp){
			if(temp->depth()==depth){
				acf.Function(temp,temp);
				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);
		}
		if(!acf.adjacencyCount){continue;}
		asf.adjacencies=new int[acf.adjacencyCount];
		asf.adjacencyCount=0;
		temp=sNodes.treeNodes[i]->nextNode();
		while(temp){
			if(temp->depth()==depth){
				asf.Function(temp,temp);
				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);
		}

		printf("\tNodes[%d/%d]: %d\n",i-sNodes.nodeCount[d]+1,sNodes.nodeCount[d+1]-sNodes.nodeCount[d],asf.adjacencyCount);
		// Get the associated vector
		SubValues.Resize(asf.adjacencyCount);
		for(j=0;j<asf.adjacencyCount;j++){SubValues[j]=Values[asf.adjacencies[j]-sNodes.nodeCount[depth]];}
		SubSolution.Resize(asf.adjacencyCount);
		for(j=0;j<asf.adjacencyCount;j++){SubSolution[j]=sNodes.treeNodes[asf.adjacencies[j]]->nodeData.value;}
		// Get the associated matrix
		SparseSymmetricMatrix<float>::Allocator.rollBack();
		GetRestrictedFixedDepthLaplacian(matrix,depth,asf.adjacencies,asf.adjacencyCount,sNodes.treeNodes[i],myRadius,sNodes);
//		gTime=Time()-gTime;
		printf("\t\tMatrix entries: %d / %d^2 = %.4f%%\n",matrix.Entries(),matrix.rows,100.0*(matrix.Entries()/double(matrix.rows))/matrix.rows);
//		DumpOutput("\t\tMemory Usage: %.3f MB\n",float(MemoryUsage()));

		// Solve the matrix
//		sTime=Time();
		iter+=SparseSymmetricMatrix<Real>::Solve(matrix,SubValues,int(pow(matrix.rows,ITERATION_POWER)),SubSolution,double(EPSILON),0);
//		sTime=Time()-sTime;

//		uTime=Time();
		LaplacianProjectionFunction lpf;
		lpf.ot=this;

		// Update the solution for all nodes in the sub-tree
		for(j=0;j<asf.adjacencyCount;j++){
			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.value=Real(SubSolution[j]);}
		}
//		double t=Time();
		// Update the values in the next depth
		int x1,x2,y1,y2,z1,z2;
		if(depth<sNodes.maxDepth-1){
			int idx1,idx2;
			TreeOctNode *node1,*node2;
			// First pass: idx2 is the solution coefficient propogated
			for(j=0;j<matrix.rows;j++){
				idx1=asf.adjacencies[j];
				node1=sNodes.treeNodes[idx1];
				if(!node1->children){continue;}
				x1=int(node1->off[0]);
				y1=int(node1->off[1]);
				z1=int(node1->off[2]);

				for(int k=0;k<matrix.rowSizes[j];k++){
					idx2=asf.adjacencies[matrix.m_ppElements[j][k].N];
					node2=sNodes.treeNodes[idx2];
					temp=node2;
					while(temp->depth()>d){temp=temp->parent;}
					if(temp!=sNodes.treeNodes[i]){continue;}
					lpf.value=Real(SubSolution[matrix.m_ppElements[j][k].N]);
					x2=int(node2->off[0]);
					y2=int(node2->off[1]);
					z2=int(node2->off[2]);
					lpf.index[0]=x2;
					lpf.index[1]=y2;
					lpf.index[2]=z2;
					dx=Real(x2-x1)/(1<<depth);
					dy=Real(y2-y1)/(1<<depth);
					dz=Real(z2-z1)/(1<<depth);
					if(fabs(dx)<myRadius2 && fabs(dy)<myRadius2 && fabs(dz)<myRadius2){node1->processNodeNodes(node2,&lpf,0);}
					else{TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,node2,width,node1,width,&lpf,0);}
				}
			}
			// Second pass: idx1 is the solution coefficient propogated
			for(j=0;j<matrix.rows;j++){
				idx1=asf.adjacencies[j];
				node1=sNodes.treeNodes[idx1];
				temp=node1;
				while(temp->depth()>d){temp=temp->parent;}
				if(temp!=sNodes.treeNodes[i]){continue;}
				x1=int(node1->off[0]);
				y1=int(node1->off[1]);
				z1=int(node1->off[2]);

				lpf.value=Real(SubSolution[j]);
				lpf.index[0]=x1;
				lpf.index[1]=y1;
				lpf.index[2]=z1;
				for(int k=0;k<matrix.rowSizes[j];k++){
					idx2=asf.adjacencies[matrix.m_ppElements[j][k].N];
					node2=sNodes.treeNodes[idx2];
					if(!node2->children){continue;}

					if(idx1!=idx2){
						x2=int(node2->off[0]);
						y2=int(node2->off[1]);
						z2=int(node2->off[2]);
						dx=Real(x1-x2)/(1<<depth);
						dy=Real(y1-y2)/(1<<depth);
						dz=Real(z1-z2)/(1<<depth);
						if(fabs(dx)<myRadius2 && fabs(dy)<myRadius2 && fabs(dz)<myRadius2){node2->processNodeNodes(node1,&lpf,0);}
						else{TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,node1,width,node2,width,&lpf,0);}
					}
				}
			}
		}
//		uTime=Time()-uTime;
		printf("\t\tGot / Solved / Updated in: %6.3f / %6.3f / %6.3f\n",gTime,sTime,uTime);
		delete[] asf.adjacencies;
	}
	return iter;
}
template<int Degree>
int Octree<Degree>::HasNormals(TreeOctNode* node,const Real& epsilon){
	int hasNormals=0;
	if(node->nodeData.nodeIndex>=0 && Length((*normals)[node->nodeData.nodeIndex])>epsilon){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){
	TreeOctNode* temp;
	temp=tree.nextNode();
	while(temp){
		if(temp->children){
			int hasNormals=0;
			for(int i=0;i<Cube::CORNERS && !hasNormals;i++){hasNormals=HasNormals(&temp->children[i],EPSILON);}
			if(!hasNormals){temp->children=NULL;}
		}
		temp=tree.nextNode(temp);
	}
}
template<int Degree>
void Octree<Degree>::SetLaplacianWeights(void){
	TreeOctNode* temp;

	fData.setDotTables(fData.DOT_FLAG | fData.D_DOT_FLAG);
	DivergenceFunction df;
	df.ot=this;
	temp=tree.nextNode();
	while(temp){
		if(temp->nodeData.nodeIndex<0 || Length((*normals)[temp->nodeData.nodeIndex])<=EPSILON){
			temp=tree.nextNode(temp);
			continue;
		}
		int d=temp->depth();
		df.normal=(*normals)[temp->nodeData.nodeIndex];
		df.index[0]=int(temp->off[0]);
		df.index[1]=int(temp->off[1]);
		df.index[2]=int(temp->off[2]);
		TreeOctNode::ProcessNodeAdjacentNodes(fData.depth,temp,width,&tree,width,&df);
		temp=tree.nextNode(temp);
	}
	fData.clearDotTables(fData.D_DOT_FLAG);
	temp=tree.nextNode();
	while(temp){
		if(temp->nodeData.nodeIndex<0){temp->nodeData.centerWeightContribution=0;}
		else{temp->nodeData.centerWeightContribution=Real(Length((*normals)[temp->nodeData.nodeIndex]));}
		temp=tree.nextNode(temp);
	}
//	MemoryUsage();

	delete normals;
	normals=NULL;
}
template<int Degree>
void Octree<Degree>::DivergenceFunction::Function(TreeOctNode* node1,const TreeOctNode* node2){
	Point3D<Real> n=normal;
	if(FunctionData<Degree,Real>::SymmetricIndex(index[0],int(node1->off[0]),scratch[0])){n.coords[0]=-n.coords[0];}
	if(FunctionData<Degree,Real>::SymmetricIndex(index[1],int(node1->off[1]),scratch[1])){n.coords[1]=-n.coords[1];}
	if(FunctionData<Degree,Real>::SymmetricIndex(index[2],int(node1->off[2]),scratch[2])){n.coords[2]=-n.coords[2];}
	double dot=ot->fData.dotTable[scratch[0]]*ot->fData.dotTable[scratch[1]]*ot->fData.dotTable[scratch[2]];
	node1->nodeData.value+=Real(dot*(ot->fData.dDotTable[scratch[0]]*n.coords[0]+ot->fData.dDotTable[scratch[1]]*n.coords[1]+ot->fData.dDotTable[scratch[2]]*n.coords[2]));
}
template<int Degree>
void Octree<Degree>::LaplacianProjectionFunction::Function(TreeOctNode* node1,const TreeOctNode* node2){
	scratch[0]=FunctionData<Degree,Real>::SymmetricIndex(index[0],int(node1->off[0]));
	scratch[1]=FunctionData<Degree,Real>::SymmetricIndex(index[1],int(node1->off[1]));
	scratch[2]=FunctionData<Degree,Real>::SymmetricIndex(index[2],int(node1->off[2]));
	node1->nodeData.value-=Real(ot->GetLaplacian(scratch)*value);
}
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;
		hash_map<long long,std::pair<RootInfo,int> >::iterator iter;
		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)){
						edges->push_back(std::pair<long long,long long>(ri2.key,ri1.key));
						iter=vertexCount->find(ri1.key);
						if(iter==vertexCount->end()){
							(*vertexCount)[ri1.key].first=ri1;
							(*vertexCount)[ri1.key].second=0;
						}
						iter=vertexCount->find(ri2.key);
						if(iter==vertexCount->end()){
							(*vertexCount)[ri2.key].first=ri2;
							(*vertexCount)[ri2.key].second=0;
						}
						(*vertexCount)[ri1.key].second--;
						(*vertexCount)[ri2.key].second++;
					}
					else{fprintf(stderr,"Bad Edge 1: %d %d\n",ri1.key,ri2.key);}
				}
			}
		}
	}
}
template<int Degree>
void Octree<Degree>::PointIndexValueFunction::Function(const TreeOctNode* node){
	int idx[DIMENSION];
	idx[0]=index[0]+int(node->off[0]);
	idx[1]=index[1]+int(node->off[1]);
	idx[2]=index[2]+int(node->off[2]);
	value+=node->nodeData.value*   Real( valueTables[idx[0]]* valueTables[idx[1]]* valueTables[idx[2]]);
}
template<int Degree>
void Octree<Degree>::PointIndexValueAndNormalFunction::Function(const TreeOctNode* node){
	int idx[DIMENSION];
	idx[0]=index[0]+int(node->off[0]);
	idx[1]=index[1]+int(node->off[1]);
	idx[2]=index[2]+int(node->off[2]);
	value+=				node->nodeData.value*   Real( valueTables[idx[0]]* valueTables[idx[1]]* valueTables[idx[2]]);
	normal.coords[0]+=	node->nodeData.value*   Real(dValueTables[idx[0]]* valueTables[idx[1]]* valueTables[idx[2]]);
	normal.coords[1]+=	node->nodeData.value*   Real( valueTables[idx[0]]*dValueTables[idx[1]]* valueTables[idx[2]]);
	normal.coords[2]+=	node->nodeData.value*   Real( valueTables[idx[0]]* valueTables[idx[1]]*dValueTables[idx[2]]);
}
template<int Degree>
int Octree<Degree>::LaplacianMatrixFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){
	Real temp;
	int d1=int(node1->d);
	int x1,y1,z1;
	x1=int(node1->off[0]);
	y1=int(node1->off[1]);
	z1=int(node1->off[2]);
	int dDepth=d2-d1;
	int d;
	d=(x2>>dDepth)-x1;
	if(d<0){return 0;}
	if(!dDepth){
		if(!d){
			d=y2-y1;
			if(d<0){return 0;}
			else if(!d){
				d=z2-z1;
				if(d<0){return 0;}
			}
		}
		scratch[0]=FunctionData<Degree,Real>::SymmetricIndex(index[0],x1);
		scratch[1]=FunctionData<Degree,Real>::SymmetricIndex(index[1],y1);
		scratch[2]=FunctionData<Degree,Real>::SymmetricIndex(index[2],z1);
		temp=ot->GetLaplacian(scratch);
		if(node1==node2){temp/=2;}
		if(fabs(temp)>EPSILON){
			rowElements[elementCount].Value=temp;
			rowElements[elementCount].N=node1->nodeData.nodeIndex-offset;
			elementCount++;
		}
		return 0;
	}
	return 1;
}

template<int Degree>
int Octree<Degree>::RestrictedLaplacianMatrixFunction::Function(const TreeOctNode* node1,const TreeOctNode* node2){
	int d1,d2,off1[3],off2[3];
	node1->depthAndOffset(d1,off1);
	node2->depthAndOffset(d2,off2);
	int dDepth=d2-d1;
	int d;
	d=(off2[0]>>dDepth)-off1[0];
	if(d<0){return 0;}

	if(!dDepth){
		if(!d){
			d=off2[1]-off1[1];
			if(d<0){return 0;}
			else if(!d){
				d=off2[2]-off1[2];
				if(d<0){return 0;}
			}
		}
		// Since we are getting the restricted matrix, we don't want to propogate out to terms that don't contribute...
		if(!TreeOctNode::Overlap2(depth,offset,0.5,d1,off1,radius)){return 0;}
		scratch[0]=FunctionData<Degree,Real>::SymmetricIndex(index[0],BinaryNode<Real>::Index(d1,off1[0]));
		scratch[1]=FunctionData<Degree,Real>::SymmetricIndex(index[1],BinaryNode<Real>::Index(d1,off1[1]));
		scratch[2]=FunctionData<Degree,Real>::SymmetricIndex(index[2],BinaryNode<Real>::Index(d1,off1[2]));
		Real temp=ot->GetLaplacian(scratch);
		if(node1==node2){temp/=2;}
		if(fabs(temp)>EPSILON){
			rowElements[elementCount].Value=temp;
			rowElements[elementCount].N=node1->nodeData.nodeIndex;
			elementCount++;
		}
		return 0;
	}
	return 1;
}

template<int Degree>
void Octree<Degree>::GetMCIsoTriangles(const Real& isoValue,CoredMeshData* mesh,const int& fullDepthIso,const int& nonLinearFit){
	double t;
	TreeOctNode* temp;

	hash_map<long long,int> roots;
	hash_map<long long,std::pair<Real,Point3D<Real> > > *normalHash=new hash_map<long long,std::pair<Real,Point3D<Real> > >();

	SetIsoSurfaceCorners(isoValue,0,fullDepthIso);
	// At the point all of the corner values have been set and all nodes are valid. Now it's just a matter
	// of running marching cubes.

//	t=Time();
	fData.setValueTables(fData.VALUE_FLAG | fData.D_VALUE_FLAG,0,postNormalSmooth);
	temp=tree.nextLeaf();
	while(temp){
		SetMCRootPositions(temp,0,isoValue,roots,NULL,*normalHash,NULL,NULL,mesh,nonLinearFit);
		temp=tree.nextLeaf(temp);
	}
//	MemoryUsage();

	printf("Normal Size: %.2f MB\n",double(sizeof(Point3D<Real>)*normalHash->size())/1000000);
//	DumpOutput("Set %d root positions in: %f\n",mesh->inCorePoints.size(),Time()-t);
//	DumpOutput("Memory Usage: %.3f MB\n",float(MemoryUsage()));

	fData.clearValueTables();
	delete normalHash;

//	DumpOutput("Post deletion size: %.3f MB\n",float(MemoryUsage()));

//	t=Time();

	// Now get the iso-surfaces, running from finest nodes to coarsest in order to allow for edge propogation from
	// finer faces to coarser ones.
	temp=tree.nextLeaf();
	while(temp){
		GetMCIsoTriangles(temp,mesh,roots,NULL,NULL,0,0);
		temp=tree.nextLeaf(temp);
	}
//	DumpOutput("Added triangles in: %f\n",Time()-t);
//	DumpOutput("Memory Usage: %.3f MB\n",float(MemoryUsage()));
}
template<int Degree>
void Octree<Degree>::GetMCIsoTriangles(const Real& isoValue,const int& subdivideDepth,CoredMeshData* mesh,const int& fullDepthIso,const int& nonLinearFit){
	TreeOctNode* temp;
	hash_map<long long,int> boundaryRoots,*interiorRoots;
	hash_map<long long,std::pair<Real,Point3D<Real> > > *boundaryNormalHash,*interiorNormalHash;
	std::vector<Point3D<float> >* interiorPoints;

	int sDepth;
	if(subdivideDepth<=0){sDepth=0;}
	else{sDepth=fData.depth-subdivideDepth;}
	if(sDepth<0){sDepth=0;}

	SetIsoSurfaceCorners(isoValue,sDepth,fullDepthIso);
	// At this point all of the corner values have been set and all nodes are valid. Now it's just a matter
	// of running marching cubes.

	boundaryNormalHash=new hash_map<long long,std::pair<Real,Point3D<Real> > >();
	int offSet=0;
	SortedTreeNodes sNodes;
	sNodes.set(tree,0);
	fData.setValueTables(fData.VALUE_FLAG | fData.D_VALUE_FLAG,0,postNormalSmooth);

	// Set the root positions for all leaf nodes below the subdivide threshold
	SetBoundaryMCRootPositions(sDepth,isoValue,boundaryRoots,*boundaryNormalHash,mesh,nonLinearFit);

	for(int i=sNodes.nodeCount[sDepth];i<sNodes.nodeCount[sDepth+1];i++){
		interiorRoots=new hash_map<long long,int>();
		interiorNormalHash=new hash_map<long long,std::pair<Real,Point3D<Real> > >();
		interiorPoints=new std::vector<Point3D<float> >();

		temp=sNodes.treeNodes[i]->nextLeaf();
		while(temp){
			if(MarchingCubes::HasRoots(temp->nodeData.mcIndex)){
				SetMCRootPositions(temp,sDepth,isoValue,boundaryRoots,interiorRoots,*boundaryNormalHash,interiorNormalHash,interiorPoints,mesh,nonLinearFit);
			}
			temp=sNodes.treeNodes[i]->nextLeaf(temp);
		}
		delete interiorNormalHash;

		temp=sNodes.treeNodes[i]->nextLeaf();
		while(temp){
			GetMCIsoTriangles(temp,mesh,boundaryRoots,interiorRoots,interiorPoints,offSet,sDepth);
			temp=sNodes.treeNodes[i]->nextLeaf(temp);
		}
		delete interiorRoots;
		delete interiorPoints;
		offSet=mesh->outOfCorePointCount();
	}
	delete boundaryNormalHash;

	temp=tree.nextLeaf();
	while(temp){
		if(temp->depth()<sDepth){GetMCIsoTriangles(temp,mesh,boundaryRoots,NULL,NULL,0,0);}
		temp=tree.nextLeaf(temp);
	}
}
template<int Degree>
Real Octree<Degree>::getCenterValue(const TreeOctNode* node){
	int idx[3];
	Real value=0;

	neighborKey2.getNeighbors(node);
	VertexData::CenterIndex(node,fData.depth,idx);
	idx[0]*=fData.res;
	idx[1]*=fData.res;
	idx[2]*=fData.res;
	for(int i=0;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=neighborKey2.neighbors[i].neighbors[j][k][l];
					if(n){
						Real temp=n->nodeData.value;
						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.value*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 TreeOctNode* node,const int& corner){
	int idx[3];
	Real value=0;

	neighborKey2.getNeighbors(node);
	VertexData::CornerIndex(node,corner,fData.depth,idx);
	idx[0]*=fData.res;
	idx[1]*=fData.res;
	idx[2]*=fData.res;
	for(int i=0;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=neighborKey2.neighbors[i].neighbors[j][k][l];
					if(n){
						Real temp=n->nodeData.value;
						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])]);
					}
				}
			}
		}
	}
	int x,y,z,d=node->depth();
	Cube::FactorCornerIndex(corner,x,y,z);
	for(int i=0;i<2;i++){
		for(int j=0;j<2;j++){
			for(int k=0;k<2;k++){
				const TreeOctNode* n=neighborKey2.neighbors[d].neighbors[x+i][y+j][z+k];
				if(n){
					int ii=Cube::AntipodalCornerIndex(Cube::CornerIndex(i,j,k));
					while(n->children){
						n=&n->children[ii];
						value+=n->nodeData.value*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])]);
					}
				}
			}
		}
	}
	return value;
}
template<int Degree>
void Octree<Degree>::getCornerValueAndNormal(const TreeOctNode* node,const int& corner,Real& value,Point3D<Real>& normal){
	int idx[3],index[3];
	value=normal.coords[0]=normal.coords[1]=normal.coords[2]=0;

	neighborKey2.getNeighbors(node);
	VertexData::CornerIndex(node,corner,fData.depth,idx);
	idx[0]*=fData.res;
	idx[1]*=fData.res;
	idx[2]*=fData.res;
	for(int i=0;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=neighborKey2.neighbors[i].neighbors[j][k][l];
					if(n){
						Real temp=n->nodeData.value;
						index[0]=idx[0]+int(n->off[0]);
						index[1]=idx[1]+int(n->off[1]);
						index[2]=idx[2]+int(n->off[2]);
						value+=temp*Real(fData.valueTables[index[0]]*fData.valueTables[index[1]]*fData.valueTables[index[2]]);
						normal.coords[0]+=temp*Real(fData.dValueTables[index[0]]* fData.valueTables[index[1]]* fData.valueTables[index[2]]);
						normal.coords[1]+=temp*Real( fData.valueTables[index[0]]*fData.dValueTables[index[1]]* fData.valueTables[index[2]]);
						normal.coords[2]+=temp*Real( fData.valueTables[index[0]]* fData.valueTables[index[1]]*fData.dValueTables[index[2]]);
					}
				}
			}
		}
	}
	int x,y,z,d=node->depth();
	Cube::FactorCornerIndex(corner,x,y,z);
	for(int i=0;i<2;i++){
		for(int j=0;j<2;j++){
			for(int k=0;k<2;k++){
				const TreeOctNode* n=neighborKey2.neighbors[d].neighbors[x+i][y+j][z+k];
				if(n){
					int ii=Cube::AntipodalCornerIndex(Cube::CornerIndex(i,j,k));
					while(n->children){
						n=&n->children[ii];
						Real temp=n->nodeData.value;
						index[0]=idx[0]+int(n->off[0]);
						index[1]=idx[1]+int(n->off[1]);
						index[2]=idx[2]+int(n->off[2]);
						value+=temp*Real(fData.valueTables[index[0]]*fData.valueTables[index[1]]*fData.valueTables[index[2]]);
						normal.coords[0]+=temp*Real(fData.dValueTables[index[0]]* fData.valueTables[index[1]]* fData.valueTables[index[2]]);
						normal.coords[1]+=temp*Real( fData.valueTables[index[0]]*fData.dValueTables[index[1]]* fData.valueTables[index[2]]);
						normal.coords[2]+=temp*Real( fData.valueTables[index[0]]* fData.valueTables[index[1]]*fData.dValueTables[index[2]]);
					}
				}
			}
		}
	}
}
template<int Degree>
Real Octree<Degree>::GetIsoValue(void){
	if(this->width<=3){
		TreeOctNode* temp;
		Real isoValue,weightSum,w;

		neighborKey2.set(fData.depth);
		fData.setValueTables(fData.VALUE_FLAG,0);

		isoValue=weightSum=0;
		temp=tree.nextNode();
		while(temp){
			w=temp->nodeData.centerWeightContribution;
			if(w>EPSILON){
				isoValue+=getCenterValue(temp)*w;
				weightSum+=w;
			}
			temp=tree.nextNode(temp);
		}
		return isoValue/weightSum;
	}
	else{
		const TreeOctNode* temp;
		Real isoValue,weightSum,w;
		Real myRadius;
		PointIndexValueFunction cf;

		fData.setValueTables(fData.VALUE_FLAG,0);
		cf.valueTables=fData.valueTables;
		cf.res2=fData.res2;
		myRadius=radius;
		isoValue=weightSum=0;
		temp=tree.nextNode();
		while(temp){
			w=temp->nodeData.centerWeightContribution;
			if(w>EPSILON){
				cf.value=0;
				int idx[3];
				VertexData::CenterIndex(temp,fData.depth,idx);
				cf.index[0]=idx[0]*fData.res;
				cf.index[1]=idx[1]*fData.res;
				cf.index[2]=idx[2]*fData.res;
				TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
				isoValue+=cf.value*w;
				weightSum+=w;
			}
			temp=tree.nextNode(temp);
		}
		return isoValue/weightSum;
	}
}
template<int Degree>
void Octree<Degree>::SetIsoSurfaceCorners(const Real& isoValue,const int& subdivideDepth,const int& fullDepthIso){
//	double t=Time();
	int i,j;
	hash_map<long long,Real> values;
	Real cornerValues[Cube::CORNERS];
	PointIndexValueFunction cf;
	TreeOctNode* temp;
	int leafCount=tree.leaves();
	long long key;
	SortedTreeNodes *sNodes=new SortedTreeNodes();
	sNodes->set(tree,0);
	temp=tree.nextNode();
	while(temp){
		temp->nodeData.mcIndex=0;
		temp=tree.nextNode(temp);
	}
	TreeNodeData::UseIndex=0;	
	// Start by setting the corner values of all the nodes
	cf.valueTables=fData.valueTables;
	cf.res2=fData.res2;
	for(i=0;i<sNodes->nodeCount[subdivideDepth];i++){
		temp=sNodes->treeNodes[i];
		if(!temp->children){
			for(j=0;j<Cube::CORNERS;j++){
				if(this->width<=3){cornerValues[j]=getCornerValue(temp,j);}
				else{
					cf.value=0;
					int idx[3];
					VertexData::CornerIndex(temp,j,fData.depth,idx);
					cf.index[0]=idx[0]*fData.res;
					cf.index[1]=idx[1]*fData.res;
					cf.index[2]=idx[2]*fData.res;
					TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
					cornerValues[j]=cf.value;
				}
			}
			temp->nodeData.mcIndex=MarchingCubes::GetIndex(cornerValues,isoValue);

			if(temp->parent){
				TreeOctNode* parent=temp->parent;
				int c=int(temp-temp->parent->children);
				int mcid=temp->nodeData.mcIndex&(1<<MarchingCubes::cornerMap[c]);
				
				if(mcid){
					parent->nodeData.mcIndex|=mcid;
					while(1){
						if(parent->parent && (parent-parent->parent->children)==c){
							parent->parent->nodeData.mcIndex|=mcid;
							parent=parent->parent;
						}
						else{break;}
					}
				}
			}
		}
	}

//	MemoryUsage();

	for(i=sNodes->nodeCount[subdivideDepth];i<sNodes->nodeCount[subdivideDepth+1];i++){
		temp=sNodes->treeNodes[i]->nextLeaf();
		while(temp){
			for(j=0;j<Cube::CORNERS;j++){
				int idx[3];
				key=VertexData::CornerIndex(temp,j,fData.depth,idx);
				cf.index[0]=idx[0]*fData.res;
				cf.index[1]=idx[1]*fData.res;
				cf.index[2]=idx[2]*fData.res;
				if(values.find(key)!=values.end()){cornerValues[j]=values[key];}
				else{
					if(this->width<=3){values[key]=cornerValues[j]=getCornerValue(temp,j);}
					else{
						cf.value=0;
						TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
						values[key]=cf.value;
						cornerValues[j]=cf.value;
					}
				}
			}
			temp->nodeData.mcIndex=MarchingCubes::GetIndex(cornerValues,isoValue);

			if(temp->parent){
				TreeOctNode* parent=temp->parent;
				int c=int(temp-temp->parent->children);
				int mcid=temp->nodeData.mcIndex&(1<<MarchingCubes::cornerMap[c]);
				
				if(mcid){
					parent->nodeData.mcIndex|=mcid;
					while(1){
						if(parent->parent && (parent-parent->parent->children)==c){
							parent->parent->nodeData.mcIndex|=mcid;
							parent=parent->parent;
						}
						else{break;}
					}
				}
			}

			temp=sNodes->treeNodes[i]->nextLeaf(temp);
		}
//		MemoryUsage();
		values.clear();
	}
	delete sNodes;
//	DumpOutput("Set corner values in: %f\n",Time()-t);
//	DumpOutput("Memory Usage: %.3f MB\n",float(MemoryUsage()));

	if(subdivideDepth){PreValidate(isoValue,fData.depth,subdivideDepth);}
}
template<int Degree>
void Octree<Degree>::Subdivide(TreeOctNode* node,const Real& isoValue,const int& maxDepth){
	int i,j,c[4];
	Real value;
	int cornerIndex2[Cube::CORNERS];
	PointIndexValueFunction cf;
	cf.valueTables=fData.valueTables;
	cf.res2=fData.res2;
	node->initChildren();
	// Since we are allocating blocks, it is possible that some of the memory was pre-allocated with
	// the wrong initialization

	// Now set the corner values for the new children
	// Copy old corner values
	for(i=0;i<Cube::CORNERS;i++){cornerIndex2[i]=node->nodeData.mcIndex&(1<<MarchingCubes::cornerMap[i]);}
	// 8 of 27 corners set

	// Set center corner
	cf.value=0;
	int idx[3];
	VertexData::CenterIndex(node,maxDepth,idx);
	cf.index[0]=idx[0]*fData.res;
	cf.index[1]=idx[1]*fData.res;
	cf.index[2]=idx[2]*fData.res;
	if(this->width<=3){value=getCenterValue(node);}
	else{
		TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
		value=cf.value;
	}
	if(value<isoValue){for(i=0;i<Cube::CORNERS;i++){cornerIndex2[i]|=1<<MarchingCubes::cornerMap[Cube::AntipodalCornerIndex(i)];}}
	// 9 of 27 set

	// Set face corners
	for(i=0;i<Cube::NEIGHBORS;i++){
		int dir,offset,e;
		Cube::FactorFaceIndex(i,dir,offset);
		cf.value=0;
		int idx[3];
		VertexData::FaceIndex(node,i,maxDepth,idx);
		cf.index[0]=idx[0]*fData.res;
		cf.index[1]=idx[1]*fData.res;
		cf.index[2]=idx[2]*fData.res;
		TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
		value=cf.value;
		Cube::FaceCorners(i,c[0],c[1],c[2],c[3]);
		e=Cube::EdgeIndex(dir,0,0);
		if(value<isoValue){for(j=0;j<4;j++){cornerIndex2[c[j]]|=1<<MarchingCubes::cornerMap[Cube::EdgeReflectCornerIndex(c[j],e)];}}
	}
	// 15 of 27 set

	// Set edge corners
	for(i=0;i<Cube::EDGES;i++){
		int o,i1,i2,f;
		Cube::FactorEdgeIndex(i,o,i1,i2);
		cf.value=0;
		int idx[3];
		VertexData::EdgeIndex(node,i,maxDepth,idx);
		cf.index[0]=idx[0]*fData.res;
		cf.index[1]=idx[1]*fData.res;
		cf.index[2]=idx[2]*fData.res;
		TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,width,&cf);
		value=cf.value;
		Cube::EdgeCorners(i,c[0],c[1]);
		f=Cube::FaceIndex(o,0);
		if(value<isoValue){for(j=0;j<2;j++){cornerIndex2[c[j]]|=1<<MarchingCubes::cornerMap[Cube::FaceReflectCornerIndex(c[j],f)];}}
	}
	// 27 of 27 set

	for(i=0;i<Cube::CORNERS;i++){node->children[i].nodeData.mcIndex=cornerIndex2[i];}
}

template<int Degree>
int Octree<Degree>::InteriorFaceRootCount(const TreeOctNode* node,const int &faceIndex,const 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,const int& edgeIndex,const 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,const int& faceIndex,const 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,const int& edgeIndex,const 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,const int& dir,const int& x,const int& y,const 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=(int(o[1])<<1) + (x<<1);
			idx2=(int(o[2])<<1) + (y<<1);
			break;
		case 1:
			idx1=(int(o[0])<<1) + (x<<1);
			idx2=(int(o[2])<<1) + (y<<1);
			break;
		case 2:
			idx1=(int(o[0])<<1) + (x<<1);
			idx2=(int(o[1])<<1) + (y<<1);
			break;
	}
	mask=2<<(int(node->d)-subdivideDepth);
	return !(idx1%(mask)) || !(idx2%(mask));
}

template<int Degree>
void Octree<Degree>::PreValidate(TreeOctNode* node,const Real& isoValue,const int& maxDepth,const int& subdivideDepth){
	int sub=0;
	if(node->children){printf("Bad Pre-Validate\n");}
//	if(int(node->d)<subdivideDepth){sub=1;}
	for(int i=0;i<Cube::NEIGHBORS && !sub;i++){
		TreeOctNode* neighbor=node->faceNeighbor(i);
		if(neighbor && neighbor->children){
			if(IsBoundaryFace(node,i,subdivideDepth) && InteriorFaceRootCount(neighbor,Cube::FaceReflectFaceIndex(i,i),maxDepth)){sub=1;}
		}
	}
	if(sub){
		Subdivide(node,isoValue,maxDepth);
		for(int i=0;i<Cube::NEIGHBORS;i++){
			if(IsBoundaryFace(node,i,subdivideDepth) && InteriorFaceRootCount(node,i,maxDepth)){
				TreeOctNode* neighbor=node->faceNeighbor(i);
				while(neighbor && !neighbor->children){
					PreValidate(neighbor,isoValue,maxDepth,subdivideDepth);
					neighbor=node->faceNeighbor(i);
				}
			}
		}
	}
}

template<int Degree>
void Octree<Degree>::PreValidate(const Real& isoValue,const int& maxDepth,const int& subdivideDepth){
	TreeOctNode* temp;

	temp=tree.nextLeaf();
	while(temp){
		PreValidate(temp,isoValue,maxDepth,subdivideDepth);
		temp=tree.nextLeaf(temp);
	}
}
template<int Degree>
void Octree<Degree>::Validate(TreeOctNode* node,const Real& isoValue,const int& maxDepth,const int& fullDepthIso){
	int i,sub=0;
	TreeOctNode* treeNode=node;
	TreeOctNode* neighbor;
	if(node->depth()>=maxDepth || node->children){return;}

	// Check if full-depth extraction is enabled and we have an iso-node that is not at maximum depth
	if(!sub && fullDepthIso && MarchingCubes::HasRoots(node->nodeData.mcIndex)){sub=1;}

	// Check if the node has faces that are ambiguous and are adjacent to finer neighbors
	for(i=0;i<Cube::NEIGHBORS && !sub;i++){
		neighbor=treeNode->faceNeighbor(i);
		if(neighbor && neighbor->children){if(MarchingCubes::IsAmbiguous(node->nodeData.mcIndex,i)){sub=1;}}
	}

	// Check if the node has edges with more than one root
	for(i=0;i<Cube::EDGES && !sub;i++){if(EdgeRootCount(node,i,maxDepth)>1){sub=1;}}

	for(i=0;i<Cube::NEIGHBORS && !sub;i++){
		neighbor=node->faceNeighbor(i);
		if(	neighbor && neighbor->children &&
			!MarchingCubes::HasFaceRoots(node->nodeData.mcIndex,i) &&
			InteriorFaceRootCount(neighbor,Cube::FaceReflectFaceIndex(i,i),maxDepth)){sub=1;}
	}
	if(sub){
		Subdivide(node,isoValue,maxDepth);
		for(i=0;i<Cube::NEIGHBORS;i++){
			neighbor=treeNode->faceNeighbor(i);
			if(neighbor && !neighbor->children){Validate(neighbor,isoValue,maxDepth,fullDepthIso);}
		}
		for(i=0;i<Cube::EDGES;i++){
			neighbor=treeNode->edgeNeighbor(i);
			if(neighbor && !neighbor->children){Validate(neighbor,isoValue,maxDepth,fullDepthIso);}
		}
		for(i=0;i<Cube::CORNERS;i++){if(!node->children[i].children){Validate(&node->children[i],isoValue,maxDepth,fullDepthIso);}}
	}
}
template<int Degree>
void Octree<Degree>::Validate(TreeOctNode* node,const Real& isoValue,const int& maxDepth,const int& fullDepthIso,const int& subdivideDepth){
	int i,sub=0;
	TreeOctNode* treeNode=node;
	TreeOctNode* neighbor;
	if(node->depth()>=maxDepth || node->children){return;}

	// Check if full-depth extraction is enabled and we have an iso-node that is not at maximum depth
	if(!sub && fullDepthIso && MarchingCubes::HasRoots(node->nodeData.mcIndex)){sub=1;}

	// Check if the node has faces that are ambiguous and are adjacent to finer neighbors
	for(i=0;i<Cube::NEIGHBORS && !sub;i++){
		neighbor=treeNode->faceNeighbor(i);
		if(neighbor && neighbor->children){if(MarchingCubes::IsAmbiguous(node->nodeData.mcIndex,i) || IsBoundaryFace(node,i,subdivideDepth)){sub=1;}}
	}

	// Check if the node has edges with more than one root
	for(i=0;i<Cube::EDGES && !sub;i++){if(EdgeRootCount(node,i,maxDepth)>1){sub=1;}}

	for(i=0;i<Cube::NEIGHBORS && !sub;i++){
		neighbor=node->faceNeighbor(i);
		if(	neighbor && neighbor->children && !MarchingCubes::HasFaceRoots(node->nodeData.mcIndex,i) &&
			InteriorFaceRootCount(neighbor,Cube::FaceReflectFaceIndex(i,i),maxDepth)){sub=1;}
	}
	if(sub){
		Subdivide(node,isoValue,maxDepth);
		for(i=0;i<Cube::NEIGHBORS;i++){
			neighbor=treeNode->faceNeighbor(i);
			if(neighbor && !neighbor->children){Validate(neighbor,isoValue,maxDepth,fullDepthIso,subdivideDepth);}
		}
		for(i=0;i<Cube::EDGES;i++){
			neighbor=treeNode->edgeNeighbor(i);
			if(neighbor && !neighbor->children){Validate(neighbor,isoValue,maxDepth,fullDepthIso,subdivideDepth);}
		}
		for(i=0;i<Cube::CORNERS;i++){if(!node->children[i].children){Validate(&node->children[i],isoValue,maxDepth,fullDepthIso,subdivideDepth);}}
	}
}
//////////////////////////////////////////////////////////////////////////////////////
// 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,const Real& isoValue,Point3D<Real> & position,hash_map<long long,std::pair<Real,Point3D<Real> > >& normalHash,const int& nonLinearFit){
	int c1,c2;
	Cube::EdgeCorners(ri.edgeIndex,c1,c2);
	if(!MarchingCubes::HasEdgeRoots(ri.node->nodeData.mcIndex,ri.edgeIndex)){return 0;}

	long long key;
	Point3D<Real> n[2];
	PointIndexValueAndNormalFunction cnf;
	cnf.valueTables=fData.valueTables;
	cnf.dValueTables=fData.dValueTables;
	cnf.res2=fData.res2;

	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 idx[3];
	key=VertexData::CornerIndex(ri.node,c1,fData.depth,idx);
	cnf.index[0]=idx[0]*fData.res;
	cnf.index[1]=idx[1]*fData.res;
	cnf.index[2]=idx[2]*fData.res;

	if(normalHash.find(key)==normalHash.end()){
		cnf.value=0;
		cnf.normal.coords[0]=cnf.normal.coords[1]=cnf.normal.coords[2]=0;
		// Careful here as the normal isn't quite accurate... (i.e. postNormalSmooth is ignored)
		if(this->width<=3){getCornerValueAndNormal(ri.node,c1,cnf.value,cnf.normal);}
		else{TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,this->width,&cnf);}
		normalHash[key]=std::pair<Real,Point3D<Real> >(cnf.value,cnf.normal);
	}
	x0=normalHash[key].first;
	n[0]=normalHash[key].second;

	key=VertexData::CornerIndex(ri.node,c2,fData.depth,idx);
	cnf.index[0]=idx[0]*fData.res;
	cnf.index[1]=idx[1]*fData.res;
	cnf.index[2]=idx[2]*fData.res;
	if(normalHash.find(key)==normalHash.end()){
		cnf.value=0;
		cnf.normal.coords[0]=cnf.normal.coords[1]=cnf.normal.coords[2]=0;
		if(this->width<=3){getCornerValueAndNormal(ri.node,c2,cnf.value,cnf.normal);}
		else{TreeOctNode::ProcessPointAdjacentNodes(fData.depth,idx,&tree,this->width,&cnf);}
		normalHash[key]=std::pair<Real,Point3D<Real> >(cnf.value,cnf.normal);
	}
	x1=normalHash[key].first;
	n[1]=normalHash[key].second;

	Point3D<Real> c;
	ri.node->centerAndWidth(c,width);
	center=c.coords[o];
	for(i=0;i<DIMENSION;i++){
		n[0].coords[i]*=width;
		n[1].coords[i]*=width;
	}

	switch(o){
				case 0:
					position.coords[1]=c.coords[1]-width/2+width*i1;
					position.coords[2]=c.coords[2]-width/2+width*i2;
					break;
				case 1:
					position.coords[0]=c.coords[0]-width/2+width*i1;
					position.coords[2]=c.coords[2]-width/2+width*i2;
					break;
				case 2:
					position.coords[0]=c.coords[0]-width/2+width*i1;
					position.coords[1]=c.coords[1]-width/2+width*i2;
					break;
	}
	double dx0,dx1;
	dx0=n[0].coords[o];
	dx1=n[1].coords[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));}

	position.coords[o]=Real(center-width/2+width*averageRoot);
	return 1;
}

template<int Degree>
int Octree<Degree>::GetRoot(const RootInfo& ri,const Real& isoValue,const int& maxDepth,Point3D<Real>& position,hash_map<long long,std::pair<Real,Point3D<Real> > >& normals,
							Point3D<Real>* normal,const int& nonLinearFit){
	if(!MarchingCubes::HasRoots(ri.node->nodeData.mcIndex)){return 0;}
	return GetRoot(ri,isoValue,position,normals,nonLinearFit);
}
template<int Degree>
int Octree<Degree>::GetRootIndex(const TreeOctNode* node,const int& edgeIndex,const int& maxDepth,const 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																							{return 0;}
	}
	else{
		if(!(MarchingCubes::edgeMask[finest->nodeData.mcIndex] & (1<<finestIndex))){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,const int& edgeIndex,const 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){
		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																				{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>::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>::GetRootPair(const RootInfo& ri,const 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 long long& key,hash_map<long long,int>& boundaryRoots,hash_map<long long,int>* interiorRoots,CoredPointIndex& index){
	hash_map<long long,int>::iterator rootIter=boundaryRoots.find(key);
	if(rootIter!=boundaryRoots.end()){
		index.inCore=1;
		index.index=rootIter->second;
		return 1;
	}
	else if(interiorRoots){
		rootIter=interiorRoots->find(key);
		if(rootIter!=interiorRoots->end()){
			index.inCore=0;
			index.index=rootIter->second;
			return 1;
		}
	}
	return 0;
}
template<int Degree>
int Octree<Degree>::SetMCRootPositions(TreeOctNode* node,const int& sDepth,const Real& isoValue,
									   hash_map<long long,int>& boundaryRoots,hash_map<long long,int>* interiorRoots,
									   hash_map<long long,std::pair<Real,Point3D<Real> > >& boundaryNormalHash,hash_map<long long,std::pair<Real,Point3D<Real> > >* interiorNormalHash,
									   std::vector<Point3D<float> >* interiorPositions,
									   CoredMeshData* mesh,const int& nonLinearFit){
	Point3D<Real> position;
	int i,j,k,eIndex;
	RootInfo ri;
	int count=0;
	if(!MarchingCubes::HasRoots(node->nodeData.mcIndex)){return 0;}
	for(i=0;i<DIMENSION;i++){
		for(j=0;j<2;j++){
			for(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(!interiorRoots || IsBoundaryEdge(node,i,j,k,sDepth)){
						if(boundaryRoots.find(key)==boundaryRoots.end()){
							GetRoot(ri,isoValue,fData.depth,position,boundaryNormalHash,NULL,nonLinearFit);
							mesh->inCorePoints.push_back(position);
							boundaryRoots[key]=int(mesh->inCorePoints.size())-1;
							count++;
						}
					}
					else{
						if(interiorRoots->find(key)==interiorRoots->end()){
							GetRoot(ri,isoValue,fData.depth,position,*interiorNormalHash,NULL,nonLinearFit);
							(*interiorRoots)[key]=mesh->addOutOfCorePoint(position);
							interiorPositions->push_back(position);
							count++;
						}
					}
				}
			}
		}
	}
	return count;
}
template<int Degree>
int Octree<Degree>::SetBoundaryMCRootPositions(const int& sDepth,const Real& isoValue,
											   hash_map<long long,int>& boundaryRoots,hash_map<long long,std::pair<Real,Point3D<Real> > >& boundaryNormalHash,
											   CoredMeshData* mesh,const int& nonLinearFit){
	Point3D<Real> position;
	int i,j,k,eIndex,hits;
	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(boundaryRoots.find(key)==boundaryRoots.end()){
									GetRoot(ri,isoValue,fData.depth,position,boundaryNormalHash,NULL,nonLinearFit);
									mesh->inCorePoints.push_back(position);
									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,hash_map<long long,int>& boundaryRoots,hash_map<long long,int>* interiorRoots,const int& sDepth,
								   std::vector<std::pair<long long,long long> >& edges){
	TreeOctNode* temp;
	int count=0,tris=0;
	int isoTri[DIMENSION*MarchingCubes::MAX_TRIANGLES];
	FaceEdgesFunction fef;
	int ref,fIndex;
	hash_map<long long,std::pair<RootInfo,int> >::iterator iter;
	hash_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)){
							edges.push_back(std::pair<long long,long long>(ri1.key,ri2.key));
							iter=vertexCount.find(ri1.key);
							if(iter==vertexCount.end()){
								vertexCount[ri1.key].first=ri1;
								vertexCount[ri1.key].second=0;
							}
							iter=vertexCount.find(ri2.key);
							if(iter==vertexCount.end()){
								vertexCount[ri2.key].first=ri2;
								vertexCount[ri2.key].second=0;
							}
							vertexCount[ri1.key].second++;
							vertexCount[ri2.key].second--;
						}
						else{fprintf(stderr,"Bad Edge 1: %d %d\n",ri1.key,ri2.key);}
					}
				}
			}
		}
	}
	for(int i=0;i<int(edges.size());i++){
		iter=vertexCount.find(edges[i].first);
		if(iter==vertexCount.end()){printf("Could not find vertex: %lld\n",edges[i].first);}
		else if(vertexCount[edges[i].first].second){
			RootInfo ri;
			GetRootPair(vertexCount[edges[i].first].first,fData.depth,ri);
			iter=vertexCount.find(ri.key);
			if(iter==vertexCount.end()){printf("Vertex pair not in list\n");}
			else{
				edges.push_back(std::pair<long long,long long>(ri.key,edges[i].first));
				vertexCount[ri.key].second++;
				vertexCount[edges[i].first].second--;
			}
		}

		iter=vertexCount.find(edges[i].second);
		if(iter==vertexCount.end()){printf("Could not find vertex: %lld\n",edges[i].second);}
		else if(vertexCount[edges[i].second].second){
			RootInfo ri;
			GetRootPair(vertexCount[edges[i].second].first,fData.depth,ri);
			iter=vertexCount.find(ri.key);
			if(iter==vertexCount.end()){printf("Vertex pair not in list\n");}
			else{
				edges.push_back(std::pair<long long,long long>(edges[i].second,ri.key));
				vertexCount[edges[i].second].second++;
				vertexCount[ri.key].second--;
			}
		}
	}
}
template<int Degree>
int Octree<Degree>::GetMCIsoTriangles(TreeOctNode* node,CoredMeshData* mesh,hash_map<long long,int>& boundaryRoots,
									  hash_map<long long,int>* interiorRoots,std::vector<Point3D<float> >* interiorPositions,const int& offSet,const int& sDepth)
{
	int tris=0;
	std::vector<std::pair<long long,long long> > edges;
	std::vector<std::vector<std::pair<long long,long long> > > edgeLoops;
	GetMCIsoEdges(node,boundaryRoots,interiorRoots,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,boundaryRoots,interiorRoots,p)){printf("Bad Point Index\n");}
			else{edgeIndices.push_back(p);}
		}
		tris+=AddTriangles(mesh,edgeIndices,interiorPositions,offSet);
	}
	return tris;
}

template<int Degree>
int Octree<Degree>::GetEdgeLoops(std::vector<std::pair<long long,long long> >& edges,std::vector<std::vector<std::pair<long long,long long> > >& loops){
	int loopSize=0;
	long long frontIdx,backIdx;
	std::pair<long long,long long> e,temp;
	loops.clear();

	while(edges.size()){
		std::vector<std::pair<long long,long long> > front,back;
		e=edges[0];
		loops.resize(loopSize+1);
		edges[0]=edges[edges.size()-1];
		edges.pop_back();
		frontIdx=e.second;
		backIdx=e.first;
		for(int j=int(edges.size())-1;j>=0;j--){
			if(edges[j].first==frontIdx || edges[j].second==frontIdx){
				if(edges[j].first==frontIdx)	{temp=edges[j];}
				else							{temp.first=edges[j].second;temp.second=edges[j].first;}
				frontIdx=temp.second;
				front.push_back(temp);
				edges[j]=edges[edges.size()-1];
				edges.pop_back();
				j=int(edges.size());
			}
			else if(edges[j].first==backIdx || edges[j].second==backIdx){
				if(edges[j].second==backIdx)	{temp=edges[j];}
				else							{temp.first=edges[j].second;temp.second=edges[j].first;}
				backIdx=temp.first;
				back.push_back(temp);
				edges[j]=edges[edges.size()-1];
				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>
int Octree<Degree>::AddTriangles(CoredMeshData* mesh,std::vector<CoredPointIndex> edges[3],std::vector<Point3D<float> >* interiorPositions,const int& offSet){
	std::vector<CoredPointIndex> e;
	for(int i=0;i<3;i++){for(size_t j=0;j<edges[i].size();j++){e.push_back(edges[i][j]);}}
	return AddTriangles(mesh,e,interiorPositions,offSet);
}
template<int Degree>
int Octree<Degree>::AddTriangles(CoredMeshData* mesh,std::vector<CoredPointIndex>& edges,std::vector<Point3D<float> >* interiorPositions,const int& offSet){
	if(edges.size()>3){
		Triangulation<float> t;

		// Add the points to the triangulation
		for(int i=0;i<int(edges.size());i++){
			Point3D<Real> p;
			if(edges[i].inCore)	{for(int j=0;j<3;j++){p.coords[j]=mesh->inCorePoints[edges[i].index].coords[j];}}
			else				{for(int j=0;j<3;j++){p.coords[j]=(*interiorPositions)[edges[i].index-offSet].coords[j];}}
			t.points.push_back(p);
		}

		// Create a fan triangulation
		for(int i=1;i<int(edges.size())-1;i++){t.addTriangle(0,i,i+1);}

		// Minimize
		while(1){
			int i;
			for(i=0;i<int(t.edges.size());i++){if(t.flipMinimize(i)){break;}}
			if(i==t.edges.size()){break;}
		}
		// Add the triangles to the mesh
		for(int i=0;i<int(t.triangles.size());i++){
			TriangleIndex tri;
			int idx[3];
			int inCoreFlag=0;
			t.factor(i,idx[0],idx[1],idx[2]);
			for(int j=0;j<3;j++){
				tri.idx[j]=edges[idx[j]].index;
				if(edges[idx[j]].inCore){inCoreFlag|=CoredMeshData::IN_CORE_FLAG[j];}
			}
			mesh->addTriangle(tri,inCoreFlag);
		}
	}
	else if(edges.size()==3){
		TriangleIndex tri;
		int inCoreFlag=0;
		for(int i=0;i<3;i++){
			tri.idx[i]=edges[i].index;
			if(edges[i].inCore){inCoreFlag|=CoredMeshData::IN_CORE_FLAG[i];}
		}
		mesh->addTriangle(tri,inCoreFlag);
	}
	return int(edges.size())-2;
}
////////////////
// VertexData //
////////////////
long long VertexData::CenterIndex(const TreeOctNode* node,const int& maxDepth){
	int idx[DIMENSION];
	return CenterIndex(node,maxDepth,idx);
}
long long VertexData::CenterIndex(const TreeOctNode* node,const 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(const int& depth,const int offSet[DIMENSION],const 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,const int& cIndex,const int& maxDepth){
	int idx[DIMENSION];
	return CornerIndex(node,cIndex,maxDepth,idx);
}
long long VertexData::CornerIndex(const TreeOctNode* node,const int& cIndex,const 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 (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
long long VertexData::CornerIndex(const int& depth,const int offSet[DIMENSION],const int& cIndex,const 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 (long long)(idx[0]) | (long long)(idx[1])<<15 | (long long)(idx[2])<<30;
}
long long VertexData::FaceIndex(const TreeOctNode* node,const int& fIndex,const int& maxDepth){
	int idx[DIMENSION];
	return FaceIndex(node,fIndex,maxDepth,idx);
}
long long VertexData::FaceIndex(const TreeOctNode* node,const int& fIndex,const 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,const int& eIndex,const int& maxDepth){
	int idx[DIMENSION];
	return EdgeIndex(node,eIndex,maxDepth,idx);
}
long long VertexData::EdgeIndex(const TreeOctNode* node,const int& eIndex,const 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;
}
