//#include <math.h>
//#define USE_VTK


#include <iostream>
#include <string>
#include <sstream>
#include <fstream>
#include <stdio.h>
#include <cmath>
#include <algorithm>
#include "math.h"
#include "meshUtils.h"
#include "fslvtkio/fslvtkio.h"

#include "utils/options.h"
#include "newimage/newimageall.h"
#include "meshclass/meshclass.h"
#include "first_lib/first_mesh.h"
#include "first_lib/first_newmat_vec.h"

//------------------------VTK STUFF------------------------//
#ifdef USE_VTK

#include "vtkPolyData.h"
#include "vtkPolyDataReader.h"

#include "vtkPolyDataMapper.h"
#include "vtkActor.h"
#include "vtkRenderer.h"
#include "vtkRenderWindow.h"
#include "vtkRenderWindowInteractor.h"

#include "vtkPoints.h"
#include "vtkProperty.h"
#include "vtkImagePlaneWidget.h"
#include "vtkImageData.h"

#include <fslvtkconv/fslvtkconv.h>

using namespace fslvtkconvname;

#endif

using namespace std;
using namespace NEWIMAGE;

using namespace mesh;
using namespace fslvtkio;
using namespace FIRST_LIB;
// Local functions
namespace meshutils {
	
	meshUtils::meshUtils(){
		
	}
	
	meshUtils::meshUtils(const string & filename,const fslvtkIO::DataType i)
{
		
		try 
	{
		switch (i)
		{
			case 0:
				setDataType(i);
				readPolyData(filename);
				break;
			case 1:
				setDataType(i);
				cout<<"read unstructured grid meshutils"<<endl;
				readUnstructuredGrid(filename);
								cout<<"end read unstructured grid"<<endl;

				break;
			default:
				throw -8;
		}
	}catch (int e)
	{
		cout<<"invalid data type"<<endl;
		return;
	}
		
}
	void meshUtils::loadMesh(const string & meshname){
		//	mesh1=new fskvtkIO;
			setDataType(static_cast<fslvtkIO::DataType>(0));
			readPolyData(meshname);
	}
	
	void meshUtils::getBounds(int *bounds,const float & xdim, const float & ydim,const float & zdim)
	{
		float xmin=1000,xmax=-1000,ymin=1000,ymax=-1000,zmin=1000,zmax=-1000;
	
		for (int i = 0; i<Points.Nrows(); i++ ){
			float tempx=Points.element(i,0);
			float tempy=Points.element(i,1);
			float tempz=Points.element(i,2);
			if (tempx<xmin){
				xmin=tempx;
			}
			if (tempx>xmax){
				xmax=tempx;
			}
			if (tempy<ymin){
				ymin=tempy;
			}
			if (tempy>ymax){
				ymax=tempy;
			}
			if (tempz<zmin){
				zmin=tempz;
			}
			if (tempz>zmax){
				zmax=tempz;
			}
		}
		*bounds=static_cast<int>(floor(xmin/xdim)-1);
		*(bounds+1)=static_cast<int>(ceil(xmax/xdim)+1);
		*(bounds+2)=static_cast<int>(floor(ymin/ydim)-1);
		*(bounds+3)=static_cast<int>(ceil(ymax/ydim)+1);
		*(bounds+4)=static_cast<int>(floor(zmin/zdim)-1);
		*(bounds+5)=static_cast<int>(ceil(zmax/zdim)+1);
		
	}
	
	void meshUtils::generateRandomMeshUsingScalar(const Mesh & min, const string & outname, const vector<bool> & scal, const int & N)
{

	srand ( time(NULL) );
	for (int i=0; i<N;i++)
	{
	
	stringstream sout;
		sout<<i;
		string sind;
		sout>>sind;
		
		Mesh m=min;
		
		int count=0;
	    for (vector<Mpoint*>::iterator i = m._points.begin(); i!=m._points.end(); i++ , count++ )
		{
			cout<<count<<" "<<scal.at(count)<<endl;
			if (scal.at(count)){
				//	cout<<static_cast<float>(rand()-RAND_MAX/2.0)<<" "<<RAND_MAX<<" "<<static_cast<float>(rand()-RAND_MAX/2.0)/RAND_MAX<<endl;
				float rn=static_cast<float>(rand()-RAND_MAX/2.0)/RAND_MAX*1.0+0.5;
				//	cout<<"rn "<<rn<<endl;
				
				(*i)->_update_coord = (*i)->get_coord() + rn * ((*i)->local_normal());
			}else
			{	
				
				//						cout<<static_cast<float>(rand()-RAND_MAX/2.0)<<" "<<RAND_MAX<<" "<<static_cast<float>(rand()-RAND_MAX/2.0)/RAND_MAX<<endl;
				
				float rn2=static_cast<float>(rand()-RAND_MAX/2.0)/RAND_MAX*1.0+0;
				//	cout<<"rn2 "<<rn2<<endl;
				
				(*i)->_update_coord = (*i)->get_coord() + rn2 * ((*i)->local_normal());
			}
		}
		m.update();
		m.save(outname+sind+".vtk",3);
	}
}

	void meshUtils::generateRandom6DOFMatrices( const string & outname, const int & N)
{
	
	const float PI=3.14159;
	//allow to go plus minus pi/4 in each direction
	
	srand ( time(NULL) );
	for (int i=0; i<N;i++)
	{
	
	stringstream sout;
		sout<<i;
		string sind;
		sout>>sind;
	
		
		float alpha=static_cast<float>(rand()-RAND_MAX/2.0)*2*PI/(4.0*RAND_MAX) ;
		float beta=static_cast<float>(rand()-RAND_MAX/2.0)*2*PI/(4.0*RAND_MAX) ;
		float gam=static_cast<float>(rand()-RAND_MAX/2.0)*2*PI/(4.0*RAND_MAX) ;

		Matrix Rx(4,4);
		Matrix Ry(4,4);
		Matrix Rz(4,4);
		
		Rx=0;
		Rx.element(0,0)=1;
		Rx.element(1,1)=cos(alpha);
		Rx.element(1,2)=sin(alpha);
		Rx.element(2,1)=-Rx.element(1,2);
		Rx.element(2,2)=Rx.element(1,1);
		Rx.element(3,3)=1;

		Ry=0;
		Ry.element(1,1)=1;
		Ry.element(0,0)=cos(beta);
		Ry.element(0,2)=-sin(beta);
		Ry.element(2,0)=-Ry.element(0,2);
		Ry.element(2,2)=Ry.element(1,1);
		Ry.element(3,3)=1;

		Rz=0;
		Rz.element(2,2)=1;
		Rz.element(0,0)=cos(gam);
		Rz.element(0,1)=sin(gam);
		Rz.element(1,0)=-Rz.element(0,1);
		Rz.element(1,1)=Rz.element(0,0);
		Rz.element(3,3)=1;


		Matrix final=Rx*Ry*Rz;
		
		float tx=static_cast<float>(rand()-RAND_MAX/2.0)*2*50/(RAND_MAX) ;
		float ty=static_cast<float>(rand()-RAND_MAX/2.0)*2*50/(RAND_MAX) ;
		float tz=static_cast<float>(rand()-RAND_MAX/2.0)*2*50/(RAND_MAX) ;

		final.element(0,3)=tx;
		final.element(1,3)=ty;
		final.element(2,3)=tz;
		ofstream fout;
		fout.open((outname+"_"+sind+".mat").c_str());
		fout<<final;
		fout.close();
	}
}

/*
		void meshUtils::getConditionalMeanAndVariance(shapemodel::shapeModel * min, volume4D<float> & iCondMean, volume4D<float> & iCondVar , const volume<float> & im, const int & mode, const float & bmin, const float & bmax, const float & res, const float & mean_offset)
{

		vector<float> vars;
		for (int i=0;i<=mode;i++)
			vars.push_back(0);
		
		volume4D<float> meanI4D;
		volume4D<float> varI4D;
		
		
		cout<<"calculate  base covariance"<<endl;
		vector< vector<float> > prec=min->getShape(0)->getICondPrec();
		vector<float> eigs=min->getShape(0)->getICondEigs();
		unsigned int N=min->getDeformedIprof(vars,mode,vars.size()).size();
			cout<<"convert vector to MAtrix"<<endl;
			Matrix Mprec=vectorOfVectorsToMatrix(prec);
cout<<"converted vector to MAtrix "<<Mprec.Nrows()<<" "<<Mprec.Ncols()<<endl;

time_t start,end;
		cout<<"time begin "<<time(&start)<<endl;
		Matrix Mcov=Mprec*Mprec.t();
		cout<<"time end"<<time(&end)<<endl;
		cout<<"time diff "<<difftime(end,start)<<endl;
		cout<<"done ultiply"<<endl;

				cout<<"time begin "<<time(&start)<<endl;
		
		for ( float b=bmin; b<bmax;b++)
			  {
				  cout<<"set mode "<<mode<<" to "<<b<<" "<<vars.size()<<endl; 
					vars.at(mode)=b;
				  cout<<"vars adjusted "<<endl;
				  vector<float> iprof=min->getDeformedIprof(vars,mode,vars.size());
				 				  
				  vector< vector<float> > vcov;
				  vector<float> var;
				
				  if ( !(eigs.size()==prec.size()) )
					  throw fslvtkIOException("number of eigenvalues does not match number of number modes");
				  
				
				volume<float> imean=im;
				volume<float> ivar=im;
				imean=0;
				ivar=0;
				float xdim=im.xdim(),ydim=im.ydim(),zdim=im.zdim() ;
				
				Mesh m= min->getDeformedMeshTrans(vars,mode,vars.size());
		
		vector<float>::iterator iprof_iter=iprof.begin();
		int ipp=min->getIPP(0);
				cout<<"ipp "<<ipp<<endl;

		cout<<"iprof "<<iprof.at(0)<<" "<<iprof.size()<<endl;
		int count=0;
		for (vector<Mpoint*>::iterator k = m._points.begin(); k!=m._points.end(); k++ )
		{
			Pt vertNew;
			Pt vert = (*k)->get_coord();
			Vec normal;
			normal = (*k)->local_normal();
			for (int j=0;j<ipp;j++,iprof_iter++,count++){
				int sc=j-(ipp-1)/2;
				vertNew=vert + normal*0.5*sc;
				//cout<<vertNew.X/xdim<<" "<<vertNew.Y/ydim<<" "<<vertNew.Z/zdim<<" "<<(*iprof_iter)+mean_offset<<endl;
				imean.value(vertNew.X/xdim,vertNew.Y/ydim,vertNew.Z/zdim)=(*iprof_iter)+mean_offset;
				ivar.value(vertNew.X/xdim,vertNew.Y/ydim,vertNew.Z/zdim)=Mprec.element(count,count);

			}
			
		}
		meanI4D.addvolume(imean);
		varI4D.addvolume(ivar);
		}

		iCondMean=meanI4D;
		iCondVar=varI4D;
}

*/
/*	void meshUtils::addModesToModelUsingMask(shapeModel * min, const vector<bool> & scal)
{

//	float mag=0;
//	for (vector<float>::const_iterator i=scal.begin();i!=scal.end();i++)
//	{
//		if (*i) mag++;
//	}
//	mag=sqrt(mag);
		
		Mesh m =min->getShapeMesh(0);
		unsigned int count=0;
	    for (vector<Mpoint*>::iterator i = m._points.begin(); i!=m._points.end(); i++ , count++ )
		{
			vector< Vec > shapeMode;
			
			if (scal.at(count)){
				vector<float> eigNew;
				eigNew.push_back(1);
				for (int j=0;j<min->getNumberOfModes();j++)
				{
					eigNew.push_back(min->getEigenValue(j));
				}	
				min->setEigenValues(eigNew);
				
				for (unsigned int j=0;j<scal.size();j++)
				{
					if (j==count) 
						shapeMode.push_back((*i)->local_normal());
					else
					{
						Vec temp(0,0,0);
						shapeMode.push_back(temp);
					}
				}
				min->getShape(0)->insertModeVector(shapeMode,0);
			}
		}
}

*/
//static function that takes in mesh class mesh
	void meshUtils::getBounds(const Mesh & m, int *bounds, const float & xdim, const float & ydim,const float & zdim){
		
		float xmin=1000,xmax=-1000,ymin=1000,ymax=-1000,zmin=1000,zmax=-1000;
		for (vector<Mpoint*>::const_iterator i = m._points.begin(); i!=m._points.end(); i++ ){
			float tempx=(*i)->get_coord().X;
			float tempy=(*i)->get_coord().Y;
			float tempz=(*i)->get_coord().Z;
			if (tempx<xmin){
				xmin=tempx;
			}
			if (tempx>xmax){
				xmax=tempx;
			}
			if (tempy<ymin){
				ymin=tempy;
			}
			if (tempy>ymax){
				ymax=tempy;
			}
			if (tempz<zmin){
				zmin=tempz;
			}
			if (tempz>zmax){
				zmax=tempz;
			}
		}
		*bounds=static_cast<int>(floor(xmin/xdim)-1);
		*(bounds+1)=static_cast<int>(ceil(xmax/xdim)+1);
		*(bounds+2)=static_cast<int>(floor(ymin/ydim)-1);
		*(bounds+3)=static_cast<int>(ceil(ymax/ydim)+1);
		*(bounds+4)=static_cast<int>(floor(zmin/zdim)-1);
		*(bounds+5)=static_cast<int>(ceil(zmax/zdim)+1);
		
	}
	
	//static
void meshUtils::fileToVector(const string & fname, vector<string> meshlist)
{
		meshlist.clear();
		ifstream fin;
		fin.open(fname.c_str());
		string meshname;
		while (fin>>meshname)
		{
			meshlist.push_back(meshname);
		}
}
vector<string> meshUtils::fileToVector(const string & fname)
{
	vector<string> meshlist;
	ifstream fin;
	fin.open(fname.c_str());
	string meshname;
	while (fin>>meshname)
	{
		meshlist.push_back(meshname);
	}
	return meshlist;
}

ReturnMatrix meshUtils::readFlirtMat(const string & fname)
{
	ifstream fin;
	fin.open(fname.c_str());
	float ftemp;
	Matrix fmat(4,4);
	for (int i =0 ; i<4;i++)
		for (int j =0 ; j<4;j++)
		{
			fin>>ftemp;
			fmat.element(i,j)=ftemp;
		}
	fmat.Release();
	return fmat;
	
}
void meshUtils::writeFlirtMatrix(const Matrix & fmat, const string & fname)
{
	ofstream fout;
	fout.open(fname.c_str());
	fout.flags( ios::fixed );
	for (int i =0 ; i<4;i++)
	{
		for (int j =0 ; j<4;j++)
			fout<<fmat.element(i,j)<<" ";
		fout<<endl;
	}
	fout.close();
}
//static for convenience
void meshUtils::meshReg(Mesh & m, const Matrix & fmat)
{
	//refsize is actually target image
	for (vector<Mpoint*>::iterator i = m._points.begin(); i!=m._points.end(); i++ )
	{
		float x=(*i)->get_coord().X;
		float y=(*i)->get_coord().Y;
		float z=(*i)->get_coord().Z;

		Pt newPt(fmat.element(0,0)*x+fmat.element(0,1)*y+fmat.element(0,2)*z+fmat.element(0,3),\
			fmat.element(1,0)*x+fmat.element(1,1)*y+fmat.element(1,2)*z+fmat.element(1,3), \
			fmat.element(2,0)*x+fmat.element(2,1)*y+fmat.element(2,2)*z+fmat.element(2,3));
		(*i)->_update_coord = newPt;
	}
	m.update();
}
void meshUtils::meshReg(const Matrix & fmat){
	ColumnVector ones(Points.Nrows());
	ones=1;
	Points=Points | ones;
	Points=(fmat*(Points.t())).t();
	Points=Points.SubMatrix(1,Points.Nrows(),1,3); 
}


	void meshUtils::shift3DVertexMatrix(Matrix & mat, const  float & tx, const float & ty, const float & tz ){
		for (int i=0; i<mat.Nrows();i++){
			mat.element(i,0)+=tx;
			mat.element(i,1)+=ty;
			mat.element(i,2)+=tz;
		}
	}
	void meshUtils::shift3DVertexColumnVector(ColumnVector & mat, const  float & tx, const float & ty, const float & tz ){
		for (int i=0; i<mat.Nrows();i+=3){
			mat.element(i)+=tx;
			mat.element(i+1)+=ty;
			mat.element(i+2)+=tz;
		}
	}
void meshUtils::shift3DMesh(Mesh & m, const float & tx, const float & ty, const float & tz )
{
	for (vector<Mpoint*>::iterator i = m._points.begin(); i!=m._points.end(); i++ )
	{
		(*i)->_update_coord.X=(*i)->get_coord().X+tx;
		(*i)->_update_coord.Y=(*i)->get_coord().Y+ty;
		(*i)->_update_coord.Z=(*i)->get_coord().Z+tz;
	}
	m.update();
	
}

	void meshUtils::shiftPoints( const  float & tx, const float & ty, const float & tz ){
		for (int i=0; i<Points.Nrows();i++){
			Points.element(i,0)+=tx;
			Points.element(i,1)+=ty;
			Points.element(i,2)+=tz;
			
		}
	}
	void meshUtils::scalePoints( const  float & sx, const float & sy, const float & sz ){
		for (int i=0; i<Points.Nrows();i++)
		{
			Points.element(i,0)*=sx;
			Points.element(i,1)*=sy;
			Points.element(i,2)*=sz;
		}
	}
	
	void meshUtils::shiftPolygonMatrix(const int & shift ){

		for (int i=0; i<Polygons.Nrows();i++)
			for (int j=0; j<Polygons.Ncols();j++)
				Polygons.element(i,j)+=shift;
		
	}
	
	ReturnMatrix meshUtils::shiftPolygonMatrix(const Matrix & mat, const int & shift ){

		Matrix polyShift(mat.Nrows(),mat.Ncols());
		for (int i=0; i<mat.Nrows();i++)
			for (int j=0; j<mat.Ncols();j++)
				polyShift.element(i,j)=mat.element(i,j)+shift;
		
		polyShift.Release();
		return polyShift;
	}
	ReturnMatrix meshUtils::meshPointsToMatrix(const Mesh & m1)
{
//this mimics the other except the reference should already be loaded 
	Matrix pointsRef(m1._points.size(),3);
	int count=0;
	for (vector<Mpoint*>::const_iterator i=m1._points.begin(); i!=m1._points.end();i++,count++)
	{
		pointsRef.element(count,0)=(*i)->get_coord().X;		
		pointsRef.element(count,1)=(*i)->get_coord().Y;		
		pointsRef.element(count,2)=(*i)->get_coord().Z;		
	}
	pointsRef.Release();
	return pointsRef;
}
	
bool meshUtils::checkLine(const float & p1, const float & p2, const float & test){
	return (((p1>test)&&(p2<test)) ||  ((p1<test)&&(p2>test)));
}


bool meshUtils::checkTriangleNeighbour(const short & tri0, const short & tri1, const short & tri2 , const short & ind0, const short & ind1, short & ind0new , short & ind1new){
	
	if (((tri0==ind0) || (tri1==ind0) || (tri2==ind0)) && ((tri0==ind1) || (tri1==ind1) || (tri2==ind1))){
		if (tri0==ind0) { ind0new=0 ; }
		else if (tri1==ind0) { ind0new=1 ;  }
		else if (tri2==ind0) { ind0new=2 ;  }
		
		
		if (tri0==ind1) { ind1new=0 ; }
		else if (tri1==ind1) { ind1new=1 ; }
		else if (tri2==ind1) { ind1new=2 ; }
		
		return true;
	}
	return false;
	
}

void meshUtils::intersectionPoint(const float & ycut, const float & px0, const float & py0, const float & pz0, const  float & dx, const float & dy, const float & dz, vector<float> & px, vector<float> & py, vector<float> & pz){
	float t=(ycut-py0)/dy;
	px.push_back(px0+dx*t);
	py.push_back(ycut);
	pz.push_back(pz0+dy*t);
	cout<<"point "<<px0+dx*t<<" "<<py0+dy*t<<" "<<pz0+dz*t<<endl;
}


float meshUtils::myatan2(const float & y, const float & x)
{
	const float PI=3.14159265358979323846;
	float f=atan2(y,x);
	/*float f=atan(y/x);
	if ((y<0)&&(x<0)) 
		f+=PI;
	else if ((y>0)&&(x<0))
		f+=PI;
	else if ((y<0)&&(x>0))
		f+=2*PI;
	*/	
	if (f<0)
		f+=2*PI;
	
	return f;
}

//assume unwrapped coordinates
ReturnMatrix  meshUtils::addSphericalCorrdinates( const Matrix & m1, const Matrix & m2 )
{
	//solves problem with avreage around 2pi/0 boundary
	//assume theta ranges from 0 to 2pi and is the second coordinate
	const float TPI=2*3.14159265358979323846;
		const float PI=3.14159265358979323846;

	Matrix Sum(m1.Nrows(),1);
	
	//we assume that problem are only with quadrant 1 and 4 operations
	for (int i=0;i<m1.Nrows();i+=3)
	{
		Sum.element(i,0)=m1.element(i,0)+m2.element(i,0);
		
		int NcircM1= static_cast<int>(floor(m1.element(i+1,0) / (TPI) ));
		if (NcircM1<0) NcircM1++;
	
		int NcircM2= static_cast<int>(floor(m2.element(i+1,0) / (TPI) ));
		if (NcircM2<0) NcircM2++;



		float remM1=m1.element(i+1,0) -NcircM1*TPI; 
		float remM2=m2.element(i+1,0) - NcircM2*TPI; 

		if (( (remM1>=0)&&(remM1<=PI/2) ) && ( (remM2>=3*PI/2)&&(remM2<=2*PI) ) )
		{
			Sum.element(i+1,0)=m1.element(i+1,0)+(m2.element(i+1,0) - (abs(NcircM2)+1)*2*PI);
			cout<<"case 1 "<<NcircM1<<" "<<NcircM2<<" "<<m1.element(i+1,0)<<" "<<m2.element(i+1,0)<<" "<<m1.element(i+1,0)+m2.element(i+1,0)<<" "<<m1.element(i+1,0)+(m2.element(i+1,0) - 2*PI)<<endl;
//		}else if ( ( (remM2>=0)&&(remM2<=PI/2) ) && ( (remM1>=3*PI/2)&&(remM1<=2*PI) ) ){
		//	Sum.element(i+1,0)=m1.element(i+1,0)-((abs(NcircM1)+1)*2*PI)+m2.element(i+1,0);
	//		cout<<"case 2 "<<NcircM1<<" "<<NcircM2<<" "<<m1.element(i+1,0)<<" "<<m2.element(i+1,0)<<" "<<m1.element(i+1,0)+m2.element(i+1,0)<<" "<<m1.element(i+1,0)+(m2.element(i+1,0) - 2*PI)<<endl;
			
		}else{
			cout<<"case 3 "<<m1.element(i+1,0)<<" "<<m2.element(i+1,0)<<" "<<m1.element(i+1,0)+m2.element(i+1,0)<<endl;
			
			Sum.element(i+1,0)=m1.element(i+1,0)+m2.element(i+1,0);//+m2.element(i+1,0);
		}
	if (Sum.element(i+1,0)<0) 
			Sum.element(i+1,0)+=2*PI;
		
		Sum.element(i+2,0)=m1.element(i+2,0)+m2.element(i+2,0);
	}
	
	Sum.Release();
	return Sum;
	
}

ReturnMatrix meshUtils::subSampleMatrix(const Matrix & m, const vector<bool> & vmask)
{
	unsigned int N=0;
	for (vector<bool>::const_iterator i=vmask.begin();i!=vmask.end();i++)
		if (*i) N++;
		
		
	Matrix subM(N,m.Ncols());
	unsigned int count=1,count2=1;
	for (vector<bool>::const_iterator i=vmask.begin();i!=vmask.end();i++,count++)
	{
		if (*i)
		{
			subM.Row(count2)=m.Row(count);
			count2++;
		}
	}
	subM.Release();
	return subM;
}


ReturnMatrix meshUtils::subSample_Nby1_3D_Matrix(const Matrix & m, const vector<bool> & vmask )
{
	unsigned int N=0;
	for (vector<bool>::const_iterator i=vmask.begin();i!=vmask.end();i++)
		if (*i) N++;
		
	Matrix subM(3*N,m.Ncols());
	unsigned int count=1,count2=1;
	for (vector<bool>::const_iterator i=vmask.begin();i!=vmask.end();i++,count+=3)
		if (*i)
		{		
		cout<<"N "<<count2<<" "<<count<<" "<<m.Nrows()<<endl;
			subM.Row(count2)=m.Row(count);
			subM.Row(count2+1)=m.Row(count+1);
			subM.Row(count2+2)=m.Row(count+2);
			count2+=3;
		}
	subM.Release();
	return subM;
}


/*
ReturnMatrix  meshUtils::averageSphericalCorrdinates( const Matrix & m1, const Matrix & m2 , int & N1, const int & N2)
{
	//theta should NEVER be outside [0,2PI[
	//solves problem with avreage around 2pi/0 boundary
	//assume theta ranges from 0 to 2pi and is the second coordinate
	const float TPI=2*3.14159265358979323846;
		const float PI=3.14159265358979323846;

	Matrix Sum(m1.Nrows(),1);
	
	//we assume that problem are only with quadrant 1 and 4 operations
	int Nnew=N1+N2;
	for (int i=0;i<m1.Nrows();i+=3)
	{
		Sum.element(i,0)=(m1.element(i,0)*N1+m2.element(i,0)*N2)/Nnew;
		Sum.element(i+2,0)=(m1.element(i+2,0)*N1+m2.element(i+2,0)*N2)/Nnew;
		cout<<"PHI "<<m1.element(i+2,0)<<" "<<m2.element(i+2,0)<<endl;


//		int NcircM1= floor(m1.element(i+1,0) / (TPI) );
//		if (NcircM1<0) NcircM1++;
	
//		int NcircM2= floor(m2.element(i+1,0) / (TPI) );
//		if (NcircM2<0) NcircM2++;
		//assumign between 0 and 2PI
		float remM1=m1.element(i+1,0);// -NcircM1*TPI; 
		float remM2=m2.element(i+1,0);// - NcircM2*TPI; 

		if (( (remM1>=0)&&(remM1<=PI/2) ) && ( (remM2>=3*PI/2.0)&&(remM2<2*PI) ) )
		{
			cout<<"HMMMMM "<<3*PI/2.0<<" "<<3*PI/2<<endl;
						Sum.element(i+1,0)=m1.element(i+1,0)*N1+(m2.element(i+1,0) -2*PI)*N2;
			cout<<"case 1 "<<NcircM1<<" "<<NcircM2<<" "<<m1.element(i+1,0)<<" "<<m2.element(i+1,0)<<" "<<m1.element(i+1,0)+m2.element(i+1,0)<<" "<<m1.element(i+1,0)+(m2.element(i+1,0) - 2*PI)<<endl;
		}else if ( ( (remM2>=0)&&(remM2<=PI/2) ) && ( (remM1>=3*PI/2.0)&&(remM1<2*PI) ) ){
		Sum.element(i+1,0)=(m1.element(i+1,0)-2*PI)*N1+m2.element(i+1,0)*N2;
			cout<<"case 2 "<<NcircM1<<" "<<NcircM2<<" "<<m1.element(i+1,0)<<" "<<m2.element(i+1,0)<<" "<<m1.element(i+1,0)+m2.element(i+1,0)<<" "<<m1.element(i+1,0)+(m2.element(i+1,0) - 2*PI)<<endl;
			
		}else{
			cout<<"case 3 "<<m1.element(i+1,0)<<" "<<m2.element(i+1,0)<<" "<<m1.element(i+1,0)+m2.element(i+1,0)<<endl;
			
			Sum.element(i+1,0)=m1.element(i+1,0)*N1+m2.element(i+1,0)*N2;//+m2.element(i+1,0);
		}
		
		Sum.element(i+1,0)/=Nnew;
		if (Sum.element(i+1,0)<0) 
			Sum.element(i+1,0)+=2*PI;
		
	}
	N1=Nnew;

	Sum.Release();
	return Sum;
	
}
*/

 void meshUtils::SVDarcSpherical( Matrix & m1, DiagonalMatrix & D, Matrix & U, Matrix & V)
{


	//takes deviattion from mean as input, arc lengths need be directional
	//converts to arc lengths
	 for (int j=0; j<m1.Ncols();j++){
		 for (int i=0; i<m1.Nrows();i+=3){
			//sign for thetha contain within theta
			m1.element(i+1,j)=m1.element(i,j)*sin(abs(m1.element(i+2,j)))*m1.element(i+1,j);
			m1.element(i+2,j)=m1.element(i,j)*m1.element(i+2,j);
		}
	}
	SVD(m1,D,U,V);
	 
	 for (int j=0; j<U.Ncols();j++){
		 for (int i=0; i<U.Nrows();i+=3){
			 //sign for thetha contain within theta
			 U.element(i+1,j)=U.element(i+1,j)/asin(abs(U.element(i+2,j)))/U.element(i,j);
			 U.element(i+2,j)/=U.element(i,j);//U.element(i+2,j);
		 }
	 }
	 
}
ReturnMatrix  meshUtils::averageSphericalCorrdinates( const Matrix & m1, const Matrix & m2 , int & N1, const int & N2)
{

	//implement the +/- PI deviation, there always a continuous average
	//theta should NEVER be outside [0,2PI[
	//solves problem with avreage around 2pi/0 boundary
	//assume theta ranges from 0 to 2pi and is the second coordinate
//	const float TPI=2*3.14159265358979323846;
		const float PI=3.14159265358979323846;

	Matrix Sum(m1.Nrows(),1);
	
	//we assume that problem are only with quadrant 1 and 4 operations
	int Nnew=N1+N2;
	for (int i=0;i<m1.Nrows();i+=3)
	{
		Sum.element(i,0)=(m1.element(i,0)*N1+m2.element(i,0)*N2)/Nnew;
		Sum.element(i+2,0)=(m1.element(i+2,0)*N1+m2.element(i+2,0)*N2)/Nnew;
		cout<<"PHI "<<m1.element(i+2,0)<<" "<<m2.element(i+2,0)<<endl;


//		int NcircM1= floor(m1.element(i+1,0) / (TPI) );
//		if (NcircM1<0) NcircM1++;
	
//		int NcircM2= floor(m2.element(i+1,0) / (TPI) );
//		if (NcircM2<0) NcircM2++;
		//assumign between 0 and 2PI
		float remM1=m1.element(i+1,0);// -NcircM1*TPI; 
		float remM2=m2.element(i+1,0);// - NcircM2*TPI; 



		if ( ((remM1>=0)&&(remM1<PI)) && (remM2>(remM1+PI)) )
		{
			Sum.element(i+1,0)=m1.element(i+1,0)*N1+(m2.element(i+1,0) -2*PI)*N2;
			cout<<"case 1 "<<" "<<m1.element(i+1,0)<<" "<<m2.element(i+1,0)<<" "<<m1.element(i+1,0)+m2.element(i+1,0)<<" "<<m1.element(i+1,0)+(m2.element(i+1,0) - 2*PI)<<endl;
		}else if ( ((remM2>=0)&&(remM2<PI)) && (remM1>(remM2+PI)) ){
		Sum.element(i+1,0)=(m1.element(i+1,0)-2*PI)*N1+m2.element(i+1,0)*N2;
			cout<<"case 2 "<<" "<<m1.element(i+1,0)<<" "<<m2.element(i+1,0)<<" "<<m1.element(i+1,0)+m2.element(i+1,0)<<" "<<m1.element(i+1,0)+(m2.element(i+1,0) - 2*PI)<<endl;
			
		}else{
			cout<<"case 3 "<<m1.element(i+1,0)<<" "<<m2.element(i+1,0)<<" "<<m1.element(i+1,0)+m2.element(i+1,0)<<endl;
			
			Sum.element(i+1,0)=m1.element(i+1,0)*N1+m2.element(i+1,0)*N2;//+m2.element(i+1,0);
		}
		
		Sum.element(i+1,0)/=Nnew;
		if (Sum.element(i+1,0)<0) 
			Sum.element(i+1,0)+=2*PI;
		
	}
	N1=Nnew;

	Sum.Release();
	return Sum;
	
}

//assume unwrapped coordinates
ReturnMatrix meshUtils::subtractSphericalCoordinates( const Matrix & m1, const Matrix & m2 )
{
	//solves problem with avreage around 2pi/0 boundary
	//solves problem with avreage around 2pi/0 boundary
	//assume theta ranges from 0 to 2pi and is the second coordinate
	const float PI=3.14159265358979323846;
	Matrix Sum(m1.Nrows(),1);
	
	//we assume that problem are only with quadrant 1 and 4 operations
	for (int i=0;i<m1.Nrows();i+=3)
	{
		Sum.element(i,0)=m1.element(i,0)+m2.element(i,0);
		if ( ( (m1.element(i+1,0)>=0)&&(m1.element(i+1,0)<=PI/2) ) && ( (m2.element(i+1,0)>=3*PI/2)&&(m2.element(i+1,0)<=2*PI) ) )
			Sum.element(i+1,0)=m1.element(i+1,0)-(m2.element(i+1,0) - 2*PI);
		else if ( ( (m2.element(i+1,0)>=0)&&(m2.element(i+1,0)<=PI/2) ) && ( (m1.element(i+1,0)>=3*PI/2)&&(m1.element(i+1,0)<=2*PI) ) )
			Sum.element(i+1,0)=(m1.element(i+1,0)-2*PI)-m2.element(i+1,0);
		else
			Sum.element(i+1,0)=m1.element(i+1,0)-m2.element(i+1,0);
		
		if (Sum.element(i+1,0)<0) 
			Sum.element(i+1,0)+=2*PI;
		
		Sum.element(i+2,0)=m1.element(i+2,0)+m2.element(i+2,0);
	}
	
	Sum.Release();
	return Sum;
}



template<class T>
void meshUtils::cartesianToSphericalCoord(vector<T> & verts)
{
	for (unsigned int i=0;i<verts.size();i+=3)
	{
		float r=sqrt(verts.at(i)*verts.at(i)+verts.at(i+1)*verts.at(i+1)+verts.at(i+2)*verts.at(i+2));
		float theta=myatan2(verts.at(i+1),verts.at(i));
		float phi=acos(verts.at(i+2)/r);
		
		verts.at(i)=r;
		verts.at(i+1)=theta;
		verts.at(i+2)=phi;
	}
}
template void meshUtils::cartesianToSphericalCoord<float>(vector<float> & verts);



template<class T>
void meshUtils::sphericalToCartesianCoord(vector<T> & verts)
{
	for (unsigned int i=0;i<verts.size();i+=3)
	{
		verts.at(i)=verts.at(i)*cos(verts.at(i+1))*sin(verts.at(i+2));
		verts.at(i+1)=verts.at(i)*sin(verts.at(i+1))*sin(verts.at(i+2));
		verts.at(i+2)=verts.at(i)*cos(verts.at(i+2));
	}
}
template void meshUtils::sphericalToCartesianCoord<float>(vector<float> & verts);


//these assumer unwrapped coordinates
void meshUtils::getSphericalCoordFromCart(NEWMAT::Matrix & Mr, NEWMAT::Matrix & Mtheta, NEWMAT::Matrix & Mphi)
{
	Mr.ReSize(Points.Nrows(),1);
	Mtheta.ReSize(Points.Nrows(),1);
	Mphi.ReSize(Points.Nrows(),1);

		for (int i=0;i<Points.Nrows();i++)
		{
			float r=sqrt(Points.element(i,0)*Points.element(i,0)+Points.element(i,1)*Points.element(i,1)+Points.element(i,2)*Points.element(i,2));
		//put theta in x/z plane
			float theta=myatan2(Points.element(i,2),Points.element(i,2));
			float phi=acos(Points.element(i,1)/r);
	//		float theta=myatan2(Points.element(i+1,j),Points.element(i,j));
	//		float phi=acos(Points.element(i+2,j)/r);
	
			
			Mr.element(i,0)=r;
			Mtheta.element(i,0)=theta;
			Mphi.element(i,0)=phi;
		}

}


void meshUtils::cartesianToSphericalCoord(Matrix & verts)
{
	for (int j=0;j<verts.Ncols();j++)
		for (int i=0;i<verts.Nrows();i+=3)
		{
			float r=sqrt(verts.element(i,j)*verts.element(i,j)+verts.element(i+1,j)*verts.element(i+1,j)+verts.element(i+2,j)*verts.element(i+2,j));
		//put theta in x/z plane
	//		float theta=myatan2(verts.element(i+2,j),verts.element(i,j));
	//		float phi=acos(verts.element(i+1,j)/r);
			float theta=myatan2(verts.element(i+1,j),verts.element(i,j));
			float phi=acos(verts.element(i+2,j)/r);
			//			cout<<"cartTOspehre "<<verts.element(i,j)<<" "<<verts.element(i+1,j)<<" "<<verts.element(i+2,j)<<" "<<theta<<" "<<theta*180/3.14<<endl;

			verts.element(i,j)=r;
			verts.element(i+1,j)=theta;
			verts.element(i+2,j)=phi;
		}
}
void meshUtils::sphericalToCartesianCoord(Matrix & verts)
{
	for (int j=0;j<verts.Ncols();j++)
		for (int i=0;i<verts.Nrows();i+=3)
		{
			verts.element(i,j)=verts.element(i,j)*cos(verts.element(i+1,j))*sin(verts.element(i+2,j));
			verts.element(i+1,j)=verts.element(i,j)*sin(verts.element(i+1,j))*sin(verts.element(i+2,j));
			verts.element(i+2,j)=verts.element(i,j)*cos(verts.element(i+2,j));
		}
}

template<class T>
void meshUtils::cartesianToSphericalCoord(Mesh & m)
{
	for (vector<Mpoint*>::iterator i=m._points.begin();i!=m._points.end();i++)
	{
		
		
		T x=(*i)->get_coord().X;
		T y=(*i)->get_coord().Y;
		T z=(*i)->get_coord().Z;
		
		T r=sqrt(x*x+y*y+z*z);
		T theta=myatan2(y,x);
		T phi=cos(z/r);
		
		(*i)->_update_coord.X=r;
		(*i)->_update_coord.Y=theta;
		(*i)->_update_coord.Z=phi;
		
		
	}
				m.update();
	
}
template void meshUtils::cartesianToSphericalCoord<float>(Mesh & m);
template void meshUtils::cartesianToSphericalCoord<double>(Mesh & m);



template<class T>
void meshUtils::sphericalToCartesianCoord(Mesh & m)
{
	for (vector<Mpoint*>::iterator i=m._points.begin();i!=m._points.end();i++)
	{
		T r=(*i)->get_coord().X;
		T theta=(*i)->get_coord().Y;
		T phi=(*i)->get_coord().Z;
		//put theta in x/z plane

//		(*i)->_update_coord.X=r*cos(theta)*sin(phi);
//		(*i)->_update_coord.Z=r*sin(theta)*sin(phi);
//		(*i)->_update_coord.Y=r*cos(phi);
		
		(*i)->_update_coord.X=r*cos(theta)*sin(phi);
		(*i)->_update_coord.Y=r*sin(theta)*sin(phi);
		(*i)->_update_coord.Z=r*cos(phi);
	}
	m.update();
}
template void meshUtils::sphericalToCartesianCoord<float>(Mesh & m);
template void meshUtils::sphericalToCartesianCoord<double>(Mesh & m);

	//-----------------------------Transformation Matrix Utilities Begin---------------------------//
	void meshUtils::preMultiplyGlobalRotation(Matrix & fmat, const Matrix & R)
	{
		//assume a 3by 3 rotation matrix
		//SubMatrix command has indices starting from 1
		fmat.SubMatrix(1,3,1,3)=R*fmat.SubMatrix(1,3,1,3);
		fmat.SubMatrix(1,3,4,4)=R*fmat.SubMatrix(1,3,4,4);
	}
	void meshUtils::preMultiplyGlobalScale(Matrix & fmat, const float & s)
	{
		for (int col=0;col<4;col++)
			for (int row=0;row<3;row++)
				fmat.element(row,col)*=s;
	}
	void meshUtils::preMultiplyGlobalScale(Matrix & fmat, const float & sx,const float & sy, const float & sz)
	{
		for (int col=0;col<4;col++)
				fmat.element(0,col)*=sx;
		for (int col=0;col<4;col++)
				fmat.element(1,col)*=sy;
		for (int col=0;col<4;col++)
				fmat.element(2,col)*=sz;
	}
	void meshUtils::preMultiplyTranslation(Matrix & fmat, const  float & tx, const float & ty, const float & tz )
	{
		//must be a 4 by 4 matrix 
		fmat.element(0,3)+=tx;
		fmat.element(1,3)+=ty;
		fmat.element(2,3)+=tz;
	}
	ReturnMatrix meshUtils::getIdentityMatrix(const short N)
	{
		Matrix fmat(N,N);
		fmat=0;
		for (int i=0;i<N;i++)
			fmat.element(i,i)=1;
		fmat.Release();
		return fmat;
	}

		//-----------------------------Tranformation Matrix Utilities End---------------------------//

void meshUtils::combineMeshesWithVectorsAndScalars(const vector<string> & meshlist)
{
	Matrix AllPoints,AllScalars,AllVectors, AllPolygons;
	for (unsigned int i=0;i<meshlist.size();i++)
	{
		loadMesh(meshlist.at(i));
		if (i==0)
		{
			AllPoints=Points;
			AllScalars=Scalars;
			AllVectors=Vectors;
			AllPolygons=Polygons;
		}else
		{
			//shift polygons call must precede pts call
			shiftPolygonMatrix(AllPoints.Nrows());
			AllPolygons=AllPolygons & Polygons;
			AllPoints=AllPoints & Points;
			AllScalars=AllScalars & Scalars;
			AllVectors=AllVectors & Vectors;
		}
	}
	Points=AllPoints;
	Scalars=AllScalars;
	Vectors=AllVectors;
	Polygons=AllPolygons;
	
	AllPoints.Release();
	AllScalars.Release();
	AllVectors.Release();
	AllPolygons.Release();
	
}






	
	void meshUtils::removeRow(Matrix & mat, int ind ){
		//operate with ind from 0 to end-1
		ind++;
		mat=mat.SubMatrix(1,ind-1,1, mat.Ncols()) & mat.SubMatrix(ind+1,mat.Nrows(),1,mat.Ncols());
		
	}


	
	
	ColumnVector meshUtils::sample_image_at_vertices(string meshname, string imagename) { 
		//setup objects
		
		
		Mesh m;
		volume<float> im;
		
		//load data
		m.load(meshname);
		read_volume(im,imagename);
		float dx=im.xdim(), dy=im.ydim(), dz=im.zdim();
		
		//variables
		int nverts=m._points.size();
		ColumnVector Samples(nverts);
		
		int count=0;
		for (vector<Mpoint*>::iterator i = m._points.begin(); i!=m._points.end(); i++ )
		{
			Samples.element(count)=im.interpolate(((*i)->get_coord().X)/dx,((*i)->get_coord().Y)/dy, ((*i)->get_coord().Z)/dz);//im.interpolate((*i)->get_coord().X, (*i)->get_coord().Y, (*i)->get_coord().Z);
			count++;
		}
		
		return Samples;
	}
	
	void meshUtils::sample_image_at_verticesNN(const string & meshname,const string & imagename,const string & outname) 
{ 
	//setup objects
	
	
	Mesh m;
	volume<float> im;
	
	//load data
	m.load(meshname);
	read_volume(im,imagename);
	float dx=im.xdim(), dy=im.ydim(), dz=im.zdim();
	
	//variables
	int nverts=m._points.size();
	ColumnVector Samples(nverts);
	
	int count=0;
	for (vector<Mpoint*>::iterator iv = m._points.begin(); iv!=m._points.end(); iv++ )
	{
		
		float val=im.interpolate(((*iv)->get_coord().X)/dx,((*iv)->get_coord().Y)/dy, ((*iv)->get_coord().Z)/dz);
		if (val!=0){
			Samples.element(count)=val;
		}else{
			float dist=1000;
			for (int i=-3;i<3;i++){
				for (int j=-3;j<3;j++){
					for (int k=-3;k<3;k++){
						float val2=im.interpolate( ((*iv)->get_coord().X+i)/dx,((*iv)->get_coord().Y+j)/dy, ((*iv)->get_coord().Z+k)/dz);
						if ((val2!=0)&&(dist<(i*i+j*j+k*k))){
							Samples.element(count)=val2;
						}
					}
				}
			}	
			
			
		}//im.interpolate((*i)->get_coord().X, (*i)->get_coord().Y, (*i)->get_coord().Z);
		count++;
	}
	fslvtkIO* output = new fslvtkIO;
	output->setMesh(m);
	output->setScalars(Samples);
	output->save(outname);
	
}



void meshUtils::sampleSumAndWrite(const string & inname, const string & inmeshname, const string & outname){
	fslvtkIO* output = new fslvtkIO;
	
	Mesh m;
	m.load(inmeshname);
	output->setMesh(m);
	
	
	
	ifstream fin;
	fin.open(inname.c_str());
	int nSub;
	fin>>nSub;
	ColumnVector meanCV;
	
	
	for (int i=0;i<nSub;i++){
		string mname;
		string iname;
		fin>>mname>>iname;
		cout<<"mname "<<mname<<" "<<iname<<endl;
		ColumnVector scalarsCV;
		scalarsCV=sample_image_at_vertices(mname, iname);
		if (i==0){
			meanCV=scalarsCV;
		}else{
			meanCV+=scalarsCV;
		}
	}
	
	//meanCV/=nSub;
	
	
	output->setScalars(meanCV);
	output->save(outname);
	
	//	ofstream scout;
	//	scout.open(outname.value().c_str());
	//	scout<<"SCALARS"<<" "<<meanCV.Nrows()<<endl;
	//	for (int i=0;i<meanCV.Nrows();i++){
	//		
	//		scout<<meanCV.element(i)<<endl;
	
	//	}
	
	//	scout.close();
	
	
}
template<class T>
void meshUtils::sampleImageAtPoints(const volume<T> & image, vector<T> & vsamples)
{
	vector<short> vmask;
	float dx=image.xdim(), dy=image.ydim(), dz=image.zdim();
//	cout<<"sample image at points "<<Points.Nrows()<<" "<<Points.Ncols()<<endl;
	for (unsigned int i=0; i<static_cast<unsigned int>(Points.Nrows());i++)
	{
			//	vsamples.push_back(static_cast<T>(image.value(Points.element(i,0)/dx, Points.element(i,1)/dy, Points.element(i,2)/dz )));
//cout<<"i "<<i<<endl;
		vsamples.push_back(static_cast<T>(image.interpolate(Points.element(i,0)/dx, Points.element(i,1)/dy, Points.element(i,2)/dz )));
	}
//	cout<<"done sampe image"<<endl;

}


template void meshUtils::sampleImageAtPoints<short>(const volume<short> & image, vector<short> & vsamples);
template void meshUtils::sampleImageAtPoints<float>(const volume<float> & image, vector<float> & vsamples);
template void meshUtils::sampleImageAtPoints<double>(const volume<double> & image, vector<double> & vsamples);


void meshUtils::LQSurfaceReg(const Matrix & refPoints, Matrix & fmat, const int & dof)
{
	//demenad vertices
//	cout<<"POINTS "<<endl;
//	cout<<Points<<endl;
	Matrix PointsDM(Points.Nrows(),Points.Ncols());
	Matrix refPointsDM(refPoints.Nrows(),refPoints.Ncols());

	//calculate centroid to determine translation 
	float mean1x=0,mean1y=0,mean1z=0;
	for (int i =0; i<Points.Nrows();i++){
	  mean1x+=Points.element(i,0);
	  mean1y+=Points.element(i,1);
	  mean1z+=Points.element(i,2);
	}
	mean1x/=Points.Nrows();
	mean1y/=Points.Nrows();
	mean1z/=Points.Nrows();


	//calculate centroid to determine translation 
	float meanRefx=0,meanRefy=0,meanRefz=0;
	for (int i =0; i<refPoints.Nrows();i++){
	  meanRefx+=refPoints.element(i,0);
	  meanRefy+=refPoints.element(i,1);
	  meanRefz+=refPoints.element(i,2);
	}

	meanRefx/=refPoints.Nrows();
	meanRefy/=refPoints.Nrows();
	meanRefz/=refPoints.Nrows();
	//we now have enough to calculate translation

	//demean data 
	//both meshes must have equal number odf vertices
	for (int i =0; i<Points.Nrows();i++){
	  PointsDM.element(i,0)=Points.element(i,0)-mean1x;
	  PointsDM.element(i,1)=Points.element(i,1)-mean1y;
	  PointsDM.element(i,2)=Points.element(i,2)-mean1z;

	  refPointsDM.element(i,0)= refPoints.element(i,0) -meanRefx;
	  refPointsDM.element(i,1)= refPoints.element(i,1) -meanRefy;
	  refPointsDM.element(i,2)= refPoints.element(i,2) -meanRefz;

	}

	//calculate translation component
	float tx=meanRefx - mean1x ;
	float ty=meanRefy - mean1y ;
	float tz=meanRefz - mean1z ;
	
	//calculate scale
	float ssq1[4]={0,0,0,0};
	float ssqRef[4]={0,0,0,0};
	float scale[4]={1,1,1,1};
	
	if (dof==7)
	{
		for (int i =0; i<Points.Nrows();i++)
			ssq1[0]+=PointsDM.element(i,0)*PointsDM.element(i,0) + PointsDM.element(i,1)*PointsDM.element(i,1) + PointsDM.element(i,2)*PointsDM.element(i,2);
		for (int i =0; i<refPoints.Nrows();i++)
			ssqRef[0]+=refPointsDM.element(i,0)*refPointsDM.element(i,0) + refPointsDM.element(i,1)*refPointsDM.element(i,1) + refPointsDM.element(i,2)*refPointsDM.element(i,2) ;
		scale[0]=sqrt(ssqRef[0]/ssq1[0]);
	}else if (dof==9)
	{
		for (int i =0; i<Points.Nrows();i++)
		{
			ssq1[1]+=PointsDM.element(i,0)*PointsDM.element(i,0);
			ssq1[2]+=PointsDM.element(i,1)*PointsDM.element(i,1);
			ssq1[3]+=PointsDM.element(i,2)*PointsDM.element(i,2);
		}
		for (int i =0; i<refPoints.Nrows();i++)
		{
			ssqRef[1]+=refPointsDM.element(i,0)*refPointsDM.element(i,0);
			ssqRef[2]+=refPointsDM.element(i,1)*refPointsDM.element(i,1);
			ssqRef[3]+=refPointsDM.element(i,2)*refPointsDM.element(i,2);
		}
		scale[1]=sqrt(ssqRef[1]/ssq1[1]);
		scale[2]=sqrt(ssqRef[2]/ssq1[2]);
		scale[3]=sqrt(ssqRef[3]/ssq1[3]);

	}
	cout<<"scale calculated "<<scale[0]<<" "<<scale[1]<<" "<<scale[2]<<" "<<scale[3]<<endl;
	//calculate rotation
	Matrix M=refPointsDM.t()*PointsDM;
	cout<<"translation "<<mean1x<<" "<<mean1y<<" "<<mean1z<<" "<<tx<<" "<<ty<<" "<<tz<<endl;
		cout<<"translation "<<meanRefx<<" "<<meanRefy<<" "<<meanRefz<<" "<<tx<<" "<<ty<<" "<<tz<<endl;

	Matrix U;
	DiagonalMatrix D;
	SVD(M.t()*M,D,U);
	//M should always be a 3x3 matrix
	for (int i=0;i<D.Nrows();i++){
	  D.element(i)=1/sqrt(D.element(i));
	}
	cout<<"Rotation Matrix ..."<<endl;
	Matrix R(3,3);
	cout<<D.Nrows()<<" "<<U.Nrows()<<" "<<U.Ncols()<<" "<<M.Nrows()<<" "<<refPointsDM.Nrows()<<" " <<refPointsDM.Ncols()<<endl; 
	R=M*(U*D*U.t());
	//R=R.t();
	cout<<"Rotation Matrix "<<R<<endl;


	fmat=getIdentityMatrix(4);
		cout<<"fmat1 "<<endl;
	cout<<fmat<<endl;
	preMultiplyTranslation(fmat,-mean1x,-mean1y,-mean1z);
	cout<<"fmat2"<<endl;
	cout<<fmat<<endl;
	if (dof==7)
		preMultiplyGlobalScale(fmat,scale[0]);
	else if (dof==9)
		preMultiplyGlobalScale(fmat,scale[1],scale[2],scale[3]);

	cout<<"fmat3"<<endl;
	cout<<fmat<<endl;
	if (dof>3)
		preMultiplyGlobalRotation(fmat,R);	
	cout<<"fmat4 "<<mean1x<<" "<<tx<<endl;
	cout<<fmat<<endl;
	preMultiplyTranslation(fmat,mean1x+tx,mean1y+ty,mean1z+tz);
		cout<<"fmat4"<<endl;
		cout<<fmat<<endl;

}

void meshUtils::findMidPointOfMidSlice(const volume<char> & im, const Matrix & fmat, float & cx, float & cy, float & cz)
{
	int bounds[6]={0,0,0,0,0,0};
	getBounds(bounds,im.xdim(),im.ydim(),im.zdim());

	cy=(bounds[2]+bounds[3])/2 * im.ydim();
	vector<float> verts=sliceMesh(im, fmat, cy); 
	
	cx=0,cz=0;
	
	float xmin=1000000,xmax=-10000, zmin=100000, zmax=-100000;
	vector<float>::iterator i=verts.begin();
	while (i!=verts.end())
	{
		if ( (*i)>xmax ) xmax=(*i);
		if ( (*i)<xmin ) xmin=(*i);
		i++;
		i++;//skip y
	
		if ( (*i)>zmax ) zmax=(*i);
		if ( (*i)<zmin ) zmin=(*i);
		i++;

	}
	cout<<xmin<<" "<<xmax<<" "<<zmin<<" "<<zmax<<endl;
	cx=(xmax+xmin)/2;
	cz=(zmax+zmin)/2;
	cout<<cx<<" "<<cy<<endl;
//this would be centroid		
//		cx+=*i; 
//		i++;
//		i++;//skip y coordinate
//		cz+=*i;
//		i++;
//	}
//	cx/=verts.size()/3;
// cz/=verts.size()/3;

}

vector<float> meshUtils::sliceMesh(const volume<char> & im, const Matrix & fmat,const float & ycut)
{
	
	//float xdim=im.xdim(),ydim=im.ydim(),zdim=im.zdim();
	
	//apply registration to space of the image or for arbitrary slicing
	meshReg(fmat);
	
	short ind0=-1, ind1=-1, indP=-1;
	vector<short> mask;
	for (int i=0; i<Polygons.Nrows();i++) //for each polygons, check if it intersect cut plane (in z)
	{
		//check each line
	  if ( checkLine(Points.element(static_cast<int>(Polygons.element(i,0)),1), Points.element(static_cast<int>(Polygons.element(i,1)),1),ycut) ) 
			mask.push_back(i);
	  else if ( checkLine(Points.element(static_cast<int>(Polygons.element(i,0)),1), Points.element(static_cast<int>(Polygons.element(i,2)),1),ycut) )
			mask.push_back(i);
	  else if ( checkLine(Points.element(static_cast<int>(Polygons.element(i,1)),1), Points.element(static_cast<int>(Polygons.element(i,2)),1),ycut) )
			mask.push_back(i);
	}
	
	
	int count=0;
	cout<<"coutn "<<count<<endl;	
	//this deterimes which line of first polygon is cut
	if (  checkLine(Points.element(static_cast<int>(Polygons.element(mask.at(0),0)),1), Points.element(static_cast<int>(Polygons.element(mask.at(0),1)),1),ycut) )
	{ 
		indP=mask.at(0); ind0=0; ind1=1;
	}
	else if (  checkLine(Points.element(static_cast<int>(Polygons.element(mask.at(0),0)),1), Points.element(static_cast<int>(Polygons.element(mask.at(0),2)),1),ycut) )
	{ 
		indP=mask.at(0); ind0=0; ind1=2; 
	}
	else if (  checkLine(Points.element(static_cast<int>(Polygons.element(mask.at(0),1)),1), Points.element(static_cast<int>(Polygons.element(mask.at(0),2)),1),ycut) )
	{
		indP=mask.at(0); ind0=1; ind1=2;
	}
	
	//these describe the line
	float px0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),0);
	float dx=Points.element(static_cast<int>(Polygons.element(indP,ind1)),0)-px0;
	float py0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),1);
	float dy=Points.element(static_cast<int>(Polygons.element(indP,ind1)),1)-py0;
	float pz0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),2);
	float dz=Points.element(static_cast<int>(Polygons.element(indP,ind1)),2)-pz0;
	
	cout<<"point "<<px0<<" "<<py0<<" "<<pz0<<" "<<dx<<" "<<dy<<" "<<dz<<" "<<ind0<<" "<<ind1<<" "<<indP<<endl;
	
	//keep track of polygon id
	vector<short> vP;
	vP.push_back(indP);
	
	vector<float> vpx,vpy,vpz; //keep track of intersection points
	intersectionPoint(ycut, px0,py0,pz0,dx,dy,dz,vpx,vpy,vpz);
	
	//found start point now find second in triangle this will define the direction of rotation
	
	if ((ind0==0)&&(ind1==1)) {
	  if ( checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut)) { 
	    ind0=0; ind1=2;
	  }
	  else if ( checkLine(Points.element(static_cast<int>(Polygons.element(indP,1)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut)) { 
	    ind0=1; ind1=2;
	  }
	}
	else if ((ind0==0)&&(ind1==2)) {
	  if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,1)),1),ycut)) {
	    ind0=0; ind1=1;  
	  }
	  else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,1)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut)) {
	    ind0=1; ind1=2;
	  }
	}
	else if ((ind0==1)&&(ind1==2)) {
	  if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,1)),1),ycut) ) {  
	    ind0=0; ind1=1; 
	  }
	  else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) ) {  
	    ind0=0; ind1=2;  
	  }	   
	}
	
	px0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),0);
	//	 dx=Points.element(Polygons.element(indP,ind1),0)-px0;
	py0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),1);
	//	 dy=Points.element(Polygons.element(indP,ind1),1)-py0;
	pz0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),2);
	//	 dz=Points.element(Polygons.element(indP,ind1),2)-pz0;
	//cout<<"point "<<px0<<" "<<py0<<" "<<pz0<<" "<<ind0<<" "<<ind1<<endl;
	intersectionPoint(ycut, px0,py0,pz0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),0)-px0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),1)-py0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),2)-pz0,vpx,vpy,vpz);
	
	//we now have the point associate with the first triangle now find negihbouring triangle
	int n=0;
	do{
		//cout<<"n "<<indP<<" "<<vP.front()<<" "<<mask.size()<<endl;
		//	cout<<ycut<<" "<<Points.element(Polygons.element(indP,ind1),1)<<" "<<Points.element(Polygons.element(indP,ind0),1)<<" "<<Polygons.element(indP,ind1)<<" "<<Polygons.element(indP,ind0)<<" "<<endl;
		
		for (vector<short>::iterator i=(mask.begin()+1); i!=mask.end(); i++){
			//	cout<<"mask "<<*i<<" "<<mask.size()<<endl;
			
			//cout<<"HMM "<<ind0<<" "<<ind1<<endl;
			//		cout<<ycut<<" "<<*i<<" "<<Points.element(Polygons.element(indP,ind1),1)<<" "<<Points.element(Polygons.element(indP,ind0),1)<<" "<<Polygons.element(indP,ind1)<<" "<<Polygons.element(indP,ind0)<<" "<<Polygons.element(*i,0)<<" "<<Polygons.element(*i,1)<<" "<<Polygons.element(*i,2)<<endl;
			//	cout<<ycut<<" "<<*i<<" "<<Polygons.element(indP,ind1)<<" "<<Polygons.element(indP,ind0)<<" "<<Polygons.element(*i,0)<<" "<<Polygons.element(*i,1)<<" "<<Polygons.element(*i,2)<<endl;
			
			short ind0new=-1, ind1new=-1;
		//	bool notused=true;
			//	cout<<"INDP "<<indP<<" "<<*i<<" "<<vP.back()<<" "<<notused<<endl;
			
			//	for (vector<short>::iterator pit=vP.begin(); pit!=vP.end(); pit++){
			//			cout<<*pit<<" ";
			//			if ((*pit)==*i){ notused=false; cout<<"notused "<<*i<<endl;}
			//		}
			//	cout<<"maskvp "<<vP.size()<<" "<<*i<<" "<<checkTriangleNeighbour(Polygons.element(*i,0),Polygons.element(*i,1),Polygons.element(*i,2), Polygons.element(indP,ind0),Polygons.element(indP,ind1) , ind0new, ind1new ) <<endl;
			
			cout<<endl;
			if  ( (checkTriangleNeighbour(static_cast<int>(Polygons.element(*i,0)),static_cast<int>(Polygons.element(*i,1)),static_cast<int>(Polygons.element(*i,2)), static_cast<int>(Polygons.element(indP,ind0)),static_cast<int>(Polygons.element(indP,ind1)) , ind0new, ind1new ) ) ){//&& ( notused)){
																																																 //	cout<<"ENTERED "<<endl;
																																																 //		cout<<"mask "<<*i<<" "<<mask.size()<<" "<<Polygons.element(indP,ind0)<<" "<<Polygons.element(indP,ind1)<<endl;
				indP=*i;
				vP.push_back(indP);
				
				mask.erase(i);
				//i--;
				
				//need to get right edge of triangle
				//	ind0=ind0new;
				//	ind1=ind1new;
				if  ( ((ind0new==0)&&(ind1new==1)) || ((ind0new==1)&&(ind1new==0)) ) {
				  if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) ) { 
				    ind0=0; ind1=2; 
				  }
				  else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,1)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) ) { 
				    ind0=1; ind1=2; 
				  }
				}
				else if ( ((ind0new==0)&&(ind1new==2)) || ((ind0new==2)&&(ind1new==0)) ) {
				  if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,1)),1),ycut) ) {
				    ind0=0; ind1=1;  
				  }
				  else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,1)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) ) {
				    ind0=1; ind1=2;  
				  }
				}
				else if ( ((ind0new==1)&&(ind1new==2)) ||  ((ind0new==2)&&(ind1new==1))  ) {
				  if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,1)),1),ycut) ) {  
				    ind0=0; ind1=1;  
				  }
				  else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) ) {  
				    ind0=0; ind1=2;  
				  }
				}			
				//			cout<<"ind01 "<<ind0<<" "<<ind1<<endl;
				px0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),0);
				py0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),1);
				pz0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),2);
				
				intersectionPoint(ycut, px0,py0,pz0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),0)-px0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),1)-py0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),2)-pz0,vpx,vpy,vpz);
				break;
			}
	}
		//	cout<<"Indp "<<indP<<endl;
		n++;
		//	cout<<"mask "<<mask.size()<<endl;
		//	cout<<"VP "<<indP<<" "<<vP.front()<<" "<<vP.size()<<" "<<mask.at(0)<<endl;	
}while (mask.size()>1);// (mask.size()>0);//(indP!=vP.front());
cout<<"outside do-while"<<endl;
//for (vector<short>::iterator pit=vP.begin(); pit!=vP.end(); pit++){
//		cout<<*pit<<" ";
//			if ((*pit)==*i){ notused=false; cout<<"notused "<<*i<<endl;}
//		}

vector<float> vVerts;
vector<float>::iterator j=vpy.begin(); 
vector<float>::iterator k=vpz.begin();
cout<<"load verts in=to vectors"<<endl; 
for (vector<float>::iterator i=vpx.begin(); i!=vpx.end();i++,j++,k++)
{
	cout<<*i<<" "<<*j<<" "<<*k<<endl;
	vVerts.push_back(*i);
	vVerts.push_back(*j);
	vVerts.push_back(*k);
	
}
return vVerts;
}



//template<class T>
vector<float> meshUtils::meshToContours(const volume<char> & im, const Matrix & fmat )
{
	
//	float xdim=im.xdim(),ydim=im.ydim(),zdim=im.zdim();
	
	//apply registration to space of the image or for arbitrary slicing
	meshReg(fmat);
	
	float minX=Points.Column(1).Minimum();
	float minY=Points.Column(2).Minimum();
	float minZ=Points.Column(3).Minimum();
	
	float maxX=Points.Column(1).Maximum();
	float maxY=Points.Column(2).Maximum();
	float maxZ=Points.Column(3).Maximum();
	
	cout<<"points bounds "<<minX<<" "<<minY<<" "<<minZ<<" "<<maxX<<" "<<maxY<<" "<<maxZ<<" "<<endl;
	//will create a contour for each image slices.
	//for (short i=floor(minY/ydim); i<floor(maxY/ydim);i++){
	
	//}
	
float testplane=(minY+maxY)/2;//to be replaced when looping
cout<<"test plane "<<testplane<<" "<<Polygons.Nrows()<<" "<<Polygons.Ncols()<<endl;
//find first polygon to intersect
float ycut=testplane;


short ind0=-1, ind1=-1, indP=-1;
vector<short> mask;
for (int i=0; i<Polygons.Nrows();i++) //for each polygons, check if it intersect cut plane (in z)
{ //check each line
  if ( checkLine(Points.element(static_cast<int>(Polygons.element(i,0)),1), Points.element(static_cast<int>(Polygons.element(i,1)),1),ycut) ) 
    mask.push_back(i);
  else if ( checkLine(Points.element(static_cast<int>(Polygons.element(i,0)),1), Points.element(static_cast<int>(Polygons.element(i,2)),1),ycut) )
    mask.push_back(i);
  else if ( checkLine(Points.element(static_cast<int>(Polygons.element(i,1)),1), Points.element(static_cast<int>(Polygons.element(i,2)),1),ycut) )
    mask.push_back(i);
}


int count=0;
cout<<"coutn "<<count<<endl;	
//this deterimes which line of first polygon is cut
if (  checkLine(Points.element(static_cast<int>(Polygons.element(mask.at(0),0)),1), Points.element(static_cast<int>(Polygons.element(mask.at(0),1)),1),ycut) ) { 
  indP=mask.at(0); ind0=0; ind1=1;
}
else if (  checkLine(Points.element(static_cast<int>(Polygons.element(mask.at(0),0)),1), Points.element(static_cast<int>(Polygons.element(mask.at(0),2)),1),ycut) ) { 
  indP=mask.at(0); ind0=0; ind1=2; 
}
else if (  checkLine(Points.element(static_cast<int>(Polygons.element(mask.at(0),1)),1), Points.element(static_cast<int>(Polygons.element(mask.at(0),2)),1),ycut) ) {
  indP=mask.at(0); ind0=1; ind1=2;
}

//these describe the line
 float px0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),0);
 float dx=Points.element(static_cast<int>(Polygons.element(indP,ind1)),0)-px0;
 float py0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),1);
 float dy=Points.element(static_cast<int>(Polygons.element(indP,ind1)),1)-py0;
 float pz0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),2);
 float dz=Points.element(static_cast<int>(Polygons.element(indP,ind1)),2)-pz0;

cout<<"point "<<px0<<" "<<py0<<" "<<pz0<<" "<<dx<<" "<<dy<<" "<<dz<<" "<<ind0<<" "<<ind1<<" "<<indP<<endl;

//keep track of polygon id
vector<short> vP;
vP.push_back(indP);

vector<float> vpx,vpy,vpz; //keep track of intersection points
intersectionPoint(ycut, px0,py0,pz0,dx,dy,dz,vpx,vpy,vpz);

//found start point now find second in triangle this will define the direction of rotation

if ((ind0==0)&&(ind1==1)) {
  if ( checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut)) { 
    ind0=0; ind1=2;
  }
  else if ( checkLine(Points.element(static_cast<int>(Polygons.element(indP,1)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut)) { 
    ind0=1; ind1=2;
  }
}
else if ((ind0==0)&&(ind1==2)) {
  if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,1)),1),ycut)) {
    ind0=0; ind1=1;  
  }
  else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,1)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut)) {
    ind0=1; ind1=2;
  }
 }
 else if ((ind0==1)&&(ind1==2)) {
   if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,1)),1),ycut) ) {  
     ind0=0; ind1=1; 
   }
   else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) ) {  
     ind0=0; ind1=2;  
   }  
 }

 px0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),0);
//	 dx=Points.element(Polygons.element(indP,ind1),0)-px0;
 py0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),1);
//	 dy=Points.element(Polygons.element(indP,ind1),1)-py0;
 pz0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),2);
//	 dz=Points.element(Polygons.element(indP,ind1),2)-pz0;
//cout<<"point "<<px0<<" "<<py0<<" "<<pz0<<" "<<ind0<<" "<<ind1<<endl;
 intersectionPoint(ycut, px0,py0,pz0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),0)-px0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),1)-py0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),2)-pz0,vpx,vpy,vpz);

//we now have the point associate with the first triangle now find negihbouring triangle
int n=0;
do{
	//cout<<"n "<<indP<<" "<<vP.front()<<" "<<mask.size()<<endl;
	//	cout<<ycut<<" "<<Points.element(Polygons.element(indP,ind1),1)<<" "<<Points.element(Polygons.element(indP,ind0),1)<<" "<<Polygons.element(indP,ind1)<<" "<<Polygons.element(indP,ind0)<<" "<<endl;
	
	for (vector<short>::iterator i=(mask.begin()+1); i!=mask.end(); i++){
		//	cout<<"mask "<<*i<<" "<<mask.size()<<endl;
		
		//cout<<"HMM "<<ind0<<" "<<ind1<<endl;
		//		cout<<ycut<<" "<<*i<<" "<<Points.element(Polygons.element(indP,ind1),1)<<" "<<Points.element(Polygons.element(indP,ind0),1)<<" "<<Polygons.element(indP,ind1)<<" "<<Polygons.element(indP,ind0)<<" "<<Polygons.element(*i,0)<<" "<<Polygons.element(*i,1)<<" "<<Polygons.element(*i,2)<<endl;
		//	cout<<ycut<<" "<<*i<<" "<<Polygons.element(indP,ind1)<<" "<<Polygons.element(indP,ind0)<<" "<<Polygons.element(*i,0)<<" "<<Polygons.element(*i,1)<<" "<<Polygons.element(*i,2)<<endl;
		
		short ind0new=-1, ind1new=-1;
	//	bool notused=true;
		//	cout<<"INDP "<<indP<<" "<<*i<<" "<<vP.back()<<" "<<notused<<endl;
		
		//	for (vector<short>::iterator pit=vP.begin(); pit!=vP.end(); pit++){
		//			cout<<*pit<<" ";
		//			if ((*pit)==*i){ notused=false; cout<<"notused "<<*i<<endl;}
		//		}
		//	cout<<"maskvp "<<vP.size()<<" "<<*i<<" "<<checkTriangleNeighbour(Polygons.element(*i,0),Polygons.element(*i,1),Polygons.element(*i,2), Polygons.element(indP,ind0),Polygons.element(indP,ind1) , ind0new, ind1new ) <<endl;
		
		cout<<endl;
		if  ( (checkTriangleNeighbour(static_cast<int>(Polygons.element(*i,0)),static_cast<int>(Polygons.element(*i,1)),static_cast<int>(Polygons.element(*i,2)), static_cast<int>(Polygons.element(indP,ind0)),static_cast<int>(Polygons.element(indP,ind1)) , ind0new, ind1new ) ) ){//&& ( notused)){
																																															 //	cout<<"ENTERED "<<endl;
																																															 //		cout<<"mask "<<*i<<" "<<mask.size()<<" "<<Polygons.element(indP,ind0)<<" "<<Polygons.element(indP,ind1)<<endl;
			indP=*i;
			vP.push_back(indP);
			
			mask.erase(i);
			//i--;
			
			//need to get right edge of triangle
			//	ind0=ind0new;
			//	ind1=ind1new;
			if  ( ((ind0new==0)&&(ind1new==1)) || ((ind0new==1)&&(ind1new==0)) )
			{
				if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) )
				{ 
					ind0=0; ind1=2; 
				}
				else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,1)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) )
				{ 
					ind0=1; ind1=2; 
				}
			}else if ( ((ind0new==0)&&(ind1new==2)) || ((ind0new==2)&&(ind1new==0)) ) 
			{
				if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,1)),1),ycut) )
				{
					ind0=0; ind1=1;  
				}
				else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,1)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) )
				{
					ind0=1; ind1=2;  
				}
			}else if ( ((ind0new==1)&&(ind1new==2)) ||  ((ind0new==2)&&(ind1new==1))  )
			{
				if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,1)),1),ycut) )
				{  
					ind0=0; ind1=1;  
				}
				else if (  checkLine(Points.element(static_cast<int>(Polygons.element(indP,0)),1), Points.element(static_cast<int>(Polygons.element(indP,2)),1),ycut) )
				{  
					ind0=0; ind1=2;  
				}
			}			
			//			cout<<"ind01 "<<ind0<<" "<<ind1<<endl;
			px0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),0);
			py0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),1);
			pz0=Points.element(static_cast<int>(Polygons.element(indP,ind0)),2);
			
			intersectionPoint(ycut, px0,py0,pz0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),0)-px0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),1)-py0,Points.element(static_cast<int>(Polygons.element(indP,ind1)),2)-pz0,vpx,vpy,vpz);
			break;
		}
	}
	//	cout<<"Indp "<<indP<<endl;
				n++;
	//	cout<<"mask "<<mask.size()<<endl;
	//	cout<<"VP "<<indP<<" "<<vP.front()<<" "<<vP.size()<<" "<<mask.at(0)<<endl;	
}while (mask.size()>1);// (mask.size()>0);//(indP!=vP.front());
cout<<"outside do-while"<<endl;
//for (vector<short>::iterator pit=vP.begin(); pit!=vP.end(); pit++){
//		cout<<*pit<<" ";
//			if ((*pit)==*i){ notused=false; cout<<"notused "<<*i<<endl;}
//		}

vector<float> vVerts;
vector<float>::iterator j=vpy.begin(); 
vector<float>::iterator k=vpz.begin();
cout<<"load verts in=to vectors"<<endl; 
for (vector<float>::iterator i=vpx.begin(); i!=vpx.end();i++,j++,k++)
{
	cout<<*i<<" "<<*j<<" "<<*k<<endl;
	vVerts.push_back(*i);
	vVerts.push_back(*j);
	vVerts.push_back(*k);
	
}
return vVerts;
}

//template vector< vector<short> > meshUtils::meshToContours(const volume<char> & im, const Matrix & fmat );
//template vector< vector<int> > meshUtils::meshToContours(const volume<char> & im, const Matrix & fmat );
//template vector< vector<float> > meshUtils::meshToContours(const volume<char> & im, const Matrix & fmat );
//template vector< vector<double> > meshUtils::meshToContours(const volume<char> & im, const Matrix & fmat );



/*
void meshUtils::meshToContours(const string & imname,const string & meshname,const string & flirtmatname,const string & outputname){
	volume<char> im;
	read_volume(im,imname);
	float xdim=im.xdim(),ydim=im.ydim(),zdim=im.zdim();
	
	fslvtkIO* min = new fslvtkIO;
	min->setDataType(static_cast<fslvtkIO::DataType>(0));
	min->readPolyData(meshname);
	
	
	//register mesh 
	//create contour in MNI space
	Matrix fmat=readFlirtMat(flirtmatname);
	meshReg(min,fmat);
	
	Matrix Points=min->getPointsAsMatrix();
	Matrix Polys=min->getPolygons();
	
	float minX=Points.Column(1).Minimum();
	float minY=Points.Column(2).Minimum();
	float minZ=Points.Column(3).Minimum();
	
	float maxX=Points.Column(1).Maximum();
	float maxY=Points.Column(2).Maximum();
	float maxZ=Points.Column(3).Maximum();
	
	cout<<"points bounds "<<minX<<" "<<minY<<" "<<minZ<<" "<<maxX<<" "<<maxY<<" "<<maxZ<<" "<<endl;
	//will create a contour for each image slices.
	//for (short i=floor(minY/ydim+0.5); i<floor(maxY/ydim+0.5);i++){
	
	//}
	
float testplane=(minY+maxY)/2;
cout<<"test plane "<<testplane<<" "<<Polys.Nrows()<<" "<<Polys.Ncols()<<endl;
//find first polygon to intersect
float ycut=testplane;
short ind0=-1, ind1=-1, indP=-1;
vector<short> mask;
for (int i=0; i<Polys.Nrows();i++){
	//check each line
	//	cout<<"i "<<i<<endl;
	
	if (  checkLine(Points.element(Polys.element(i,0),1), Points.element(Polys.element(i,1),1),ycut) ){ mask.push_back(i) ; }
	else if (  checkLine(Points.element(Polys.element(i,0),1), Points.element(Polys.element(i,2),1),ycut) ){  mask.push_back(i) ; }
	else if (  checkLine(Points.element(Polys.element(i,1),1), Points.element(Polys.element(i,2),1),ycut) ){ mask.push_back(i)  ;  }
	//	cout<<"mask "<<mask.size()<<endl;
	//	if (mask.size()>0) cout<<mask.at(mask.size()-1)<<endl;
}
for (vector<short>::iterator i=mask.begin(); i!=mask.end(); i++){
				cout<<"M "<<Polys.element(*i,0)<<" "<<Polys.element(*i,1)<<" "<<Polys.element(*i,2)<<endl;
	
}

int count=0;
//	for (vector<short>::iterator i=mask.begin(); i!=mask.end(); i++){
//check each line
cout<<"coutn "<<count<<endl;	

if (  checkLine(Points.element(Polys.element(mask.at(0),0),1), Points.element(Polys.element(mask.at(0),1),1),ycut) ){ indP=mask.at(0); ind0=0; ind1=1;  }
else if (  checkLine(Points.element(Polys.element(mask.at(0),0),1), Points.element(Polys.element(mask.at(0),2),1),ycut) ){ indP=mask.at(0); ind0=0; ind1=2; }
else if (  checkLine(Points.element(Polys.element(mask.at(0),1),1), Points.element(Polys.element(mask.at(0),2),1),ycut) ){indP=mask.at(0); ind0=1; ind1=2;  }
//	}

float px0=Points.element(Polys.element(indP,ind0),0);
float dx=Points.element(Polys.element(indP,ind1),0)-px0;
float py0=Points.element(Polys.element(indP,ind0),1);
float dy=Points.element(Polys.element(indP,ind1),1)-py0;
float pz0=Points.element(Polys.element(indP,ind0),2);
float dz=Points.element(Polys.element(indP,ind1),2)-pz0;
cout<<"point "<<px0<<" "<<py0<<" "<<pz0<<" "<<dx<<" "<<dy<<" "<<dz<<" "<<ind0<<" "<<ind1<<" "<<indP<<endl;

vector<short> vP;
vP.push_back(indP);

vector<float> vpx,vpy,vpz;

intersectionPoint(ycut, px0,py0,pz0,dx,dy,dz,vpx,vpy,vpz);


//found start point no find second in triangle this will define the direction of rotation

if ((ind0==0)&&(ind1==1)){
	if (  checkLine(Points.element(Polys.element(indP,0),1), Points.element(Polys.element(indP,2),1),ycut) ){ ind0=0; ind1=2; }
	else if (  checkLine(Points.element(Polys.element(indP,1),1), Points.element(Polys.element(indP,2),1),ycut) ){ ind0=1; ind1=2; }
}else if ((ind0==0)&&(ind1==2)){
	if (  checkLine(Points.element(Polys.element(indP,0),1), Points.element(Polys.element(indP,1),1),ycut) ){ind0=0; ind1=1;  }
	else if (  checkLine(Points.element(Polys.element(indP,1),1), Points.element(Polys.element(indP,2),1),ycut) ){ind0=1; ind1=2;  }
}else if ((ind0==1)&&(ind1==2)){
	if (  checkLine(Points.element(Polys.element(indP,0),1), Points.element(Polys.element(indP,1),1),ycut) ){  ind0=0; ind1=1;  }
	else if (  checkLine(Points.element(Polys.element(indP,0),1), Points.element(Polys.element(indP,2),1),ycut) ){  ind0=0; ind1=2;  }
}	


px0=Points.element(Polys.element(indP,ind0),0);
//	 dx=Points.element(Polys.element(indP,ind1),0)-px0;
py0=Points.element(Polys.element(indP,ind0),1);
//	 dy=Points.element(Polys.element(indP,ind1),1)-py0;
pz0=Points.element(Polys.element(indP,ind0),2);
//	 dz=Points.element(Polys.element(indP,ind1),2)-pz0;
cout<<"point "<<px0<<" "<<py0<<" "<<pz0<<" "<<ind0<<" "<<ind1<<endl;
intersectionPoint(ycut, px0,py0,pz0,Points.element(Polys.element(indP,ind1),0)-px0,Points.element(Polys.element(indP,ind1),1)-py0,Points.element(Polys.element(indP,ind1),2)-pz0,vpx,vpy,vpz);

//intersectionPoint(ycut, px0,py0,pz0,dx,dy,dz,vpx,vpy,vpz);
cout<<"maks: "<<mask.size()<<endl;

//	mask.erase(mask.begin());
cout<<"maks: "<<mask.size()<<endl;





//we now have the point associate with the first triangle now find negihbouring triangle
int n=0;
do{
	cout<<"n "<<indP<<" "<<vP.front()<<" "<<mask.size()<<endl;
	cout<<ycut<<" "<<Points.element(Polys.element(indP,ind1),1)<<" "<<Points.element(Polys.element(indP,ind0),1)<<" "<<Polys.element(indP,ind1)<<" "<<Polys.element(indP,ind0)<<" "<<endl;
	
	for (vector<short>::iterator i=mask.begin(); i!=mask.end(); i++){
		//cout<<"HMM "<<ind0<<" "<<ind1<<endl;
		//		cout<<ycut<<" "<<*i<<" "<<Points.element(Polys.element(indP,ind1),1)<<" "<<Points.element(Polys.element(indP,ind0),1)<<" "<<Polys.element(indP,ind1)<<" "<<Polys.element(indP,ind0)<<" "<<Polys.element(*i,0)<<" "<<Polys.element(*i,1)<<" "<<Polys.element(*i,2)<<endl;
		cout<<ycut<<" "<<*i<<" "<<Polys.element(indP,ind1)<<" "<<Polys.element(indP,ind0)<<" "<<Polys.element(*i,0)<<" "<<Polys.element(*i,1)<<" "<<Polys.element(*i,2)<<endl;
		
		short ind0new=-1, ind1new=-1;
		bool notused=true;
		for (vector<short>::iterator pit=vP.begin(); pit!=vP.end(); pit++){
			if ((*pit)==*i) notused=false;
		}
								cout<<"INDP "<<indP<<" "<<*i<<" "<<vP.back()<<" "<<notused<<endl;
		
		if  ( (checkTriangleNeighbour(Polys.element(*i,0),Polys.element(*i,1),Polys.element(*i,2), Polys.element(indP,ind0),Polys.element(indP,ind1) , ind0new, ind1new ) )
			  && ( notused)){
			cout<<"ENTERED "<<endl;
			cout<<"mask "<<*i<<" "<<mask.size()<<" "<<Polys.element(indP,ind0)<<" "<<Polys.element(indP,ind1)<<endl;
			indP=*i;
			vP.push_back(indP);
			
			//mask.erase(i);
			
			//							cout<<"mask "<<mask.size()<<endl;
			
			for (vector<short>::iterator i=mask.begin(); i!=mask.end(); i++){
				cout<<"M "<<Polys.element(*i,0)<<" "<<Polys.element(*i,1)<<" "<<Polys.element(*i,2)<<endl;
				
			}
			
			
			//need to get right edge of triangle
			//	ind0=ind0new;
			//	ind1=ind1new;
			if  ( ((ind0new==0)&&(ind1new==1)) || ((ind0new==1)&&(ind1new==0)) ){
				if (  checkLine(Points.element(Polys.element(indP,0),1), Points.element(Polys.element(indP,2),1),ycut) ){ ind0=0; ind1=2; }
				else if (  checkLine(Points.element(Polys.element(indP,1),1), Points.element(Polys.element(indP,2),1),ycut) ){ ind0=1; ind1=2; }
			}else if ( ((ind0new==0)&&(ind1new==2)) || ((ind0new==2)&&(ind1new==0)) ) {
				if (  checkLine(Points.element(Polys.element(indP,0),1), Points.element(Polys.element(indP,1),1),ycut) ){ind0=0; ind1=1;  }
				else if (  checkLine(Points.element(Polys.element(indP,1),1), Points.element(Polys.element(indP,2),1),ycut) ){ind0=1; ind1=2;  }
			}else if ( ((ind0new==1)&&(ind1new==2)) ||  ((ind0new==2)&&(ind1new==1))  ){
				if (  checkLine(Points.element(Polys.element(indP,0),1), Points.element(Polys.element(indP,1),1),ycut) ){  ind0=0; ind1=1;  }
				else if (  checkLine(Points.element(Polys.element(indP,0),1), Points.element(Polys.element(indP,2),1),ycut) ){  ind0=0; ind1=2;  }
			}				
			cout<<"ind01 "<<ind0<<" "<<ind1<<endl;
			px0=Points.element(Polys.element(indP,ind0),0);
			py0=Points.element(Polys.element(indP,ind0),1);
			pz0=Points.element(Polys.element(indP,ind0),2);
			
			intersectionPoint(ycut, px0,py0,pz0,Points.element(Polys.element(indP,ind1),0)-px0,Points.element(Polys.element(indP,ind1),1)-py0,Points.element(Polys.element(indP,ind1),2)-pz0,vpx,vpy,vpz);
			break;
		}
	}
	//	cout<<"Indp "<<indP<<endl;
				n++;
	
	cout<<"VP "<<vP.size()<<endl;
	
}while (n<70);//(indP!=vP.front());


//	px.push_back();





}
*/



void meshUtils::draw_segment(volume<short>& image, const Pt& p1, const Pt& p2, int label)
{
	double xdim = (double) image.xdim();
	double ydim = (double) image.ydim();
	double zdim = (double) image.zdim();
	
	//in new version of bet2
	double mininc = min(xdim,min(ydim,zdim)) * .5;
	
	
	
	Vec n = (p1 - p2);
	double d = n.norm();
	n.normalize();
	//	double l = d*4;
	for (double i=0; i<=d; i+=mininc)
    {
		Pt p = p2 + i* n;
		image((int) floor((p.X)/xdim +.5),(int) floor((p.Y)/ydim +.5),(int) floor((p.Z)/zdim +.5)) = label;
    }
}


volume<short> meshUtils::draw_mesh(const volume<short>& image, const Mesh &m, int label)
{
	
	double xdim = (double) image.xdim();
	double ydim = (double) image.ydim();
	double zdim = (double) image.zdim();
	
	//in new version of bet2
	double mininc = min(xdim,min(ydim,zdim)) * .5;
	
	
	volume<short> res = image;
	for (list<Triangle*>::const_iterator i = m._triangles.begin(); i!=m._triangles.end(); i++)
    {
		Vec n = (*(*i)->get_vertice(0) - *(*i)->get_vertice(1));
		double d = n.norm();
		n.normalize();
	       
		
		for (double j=0; j<=d ;  j+=mininc)
		{
			Pt p = (*i)->get_vertice(1)->get_coord()  + (double)j* n;
			draw_segment(res, p, (*i)->get_vertice(2)->get_coord(),label);
		} 
    }
	return res;
}

volume<short> meshUtils::make_mask_from_mesh(const volume<float> & image, const Mesh& m, int label, int* bounds, const bool & sep_boundary)
{
	
	float xdim = (float) image.xdim();
	float ydim = (float) image.ydim();
	float zdim = (float) image.zdim();
	
	volume<short> mask;
	copyconvert(image,mask);
	
	
	mask = 0;
	if (sep_boundary)
		mask = draw_mesh(mask, m,label+100);
	else
		mask = draw_mesh(mask, m,label);

	
	// THIS EXCLUDEDS THE ACTUAL MESH
	volume<short> otl=mask;
	getBounds(m,bounds,xdim,ydim,zdim);
	vector<Pt> current;
	current.clear();
	Pt c(bounds[0]-2, bounds[2]-2, bounds[4]-2);
	
	mask.value(static_cast<int>(c.X),static_cast<int>(c.Y),static_cast<int>(c.Z)) = label;
	current.push_back(c);
	int fillCount=0;
	while (!current.empty())
	{
		Pt pc = current.back();
		int x, y, z;
		x=(int) pc.X; y=(int) pc.Y; z=(int) pc.Z;
		
		current.pop_back();
		fillCount++;
		
		
		if (bounds[0]<=x-1 && mask.value(x-1, y, z)==0) {
			mask.value(x-1, y, z) = label;
			current.push_back(Pt(x-1, y, z));
		}
		if (bounds[2]<=y-1 && mask.value(x, y-1, z)==0) {
			mask.value(x, y-1, z) = label;
			current.push_back(Pt(x, y-1, z));
		}
		if (bounds[4]<=z-1 && mask.value(x, y, z-1)==0) {
			mask.value(x, y, z-1) = label;
			current.push_back(Pt(x, y, z-1));
		}
		if (bounds[1]>=x+1 && mask.value(x+1, y, z)==0){
			mask.value(x+1, y, z) = label;
			current.push_back(Pt(x+1, y, z));
		}
		if (bounds[3]>=y+1 && mask.value(x, y+1, z)==0){
			mask.value(x, y+1, z) = label;
			current.push_back(Pt(x, y+1, z));
		}
		if (bounds[5]>=z+1 && mask.value(x, y, z+1)==0){
			mask.value(x, y, z+1) = label;
			current.push_back(Pt(x, y, z+1)); 
		}
		
	}
	for (int i=bounds[0];i<bounds[1];i++){
		for (int j=bounds[2];j<bounds[3];j++){
			for (int k=bounds[4];k<bounds[5];k++){
				if (mask.value(i,j,k)==0){
					otl.value(i,j,k)=label;
				}
			}
		}
	}
	
	return otl;
	//return mask;
	
}



void meshUtils::fillMesh(const string & imname, const string & meshname, const string & outname, const int & label, const bool & sep_bound ){
	volume<float> im;
	read_volume(im,imname);
	
	Mesh m;
	m.load(meshname);
	
	int bounds[6];
	
	volume<short> imout;
	imout=make_mask_from_mesh(im, m, label, bounds,sep_bound);
	
	save_volume(imout,outname);
	
}


double meshUtils::maxScalar()
{
	double max=-10e-6;
	for (unsigned int i=0; i<static_cast<unsigned int>(Scalars.Nrows());i++)
	{
	//  cout<<"i "<<i<<" "<<Scalars.Nrows()<<" "<<Scalars.Ncols()<<endl;
	  if (Scalars.element(i,0)>max)
			max=Scalars.element(i,0);
	//		cout<<"max "<<max<<endl;

	}	
	return max;		
}


double meshUtils::meanScalar()
{
	double mean=0;
	for (unsigned int i=0; i<(unsigned)Scalars.Nrows();i++)
	{
		mean+=Scalars.element(i,0);
	
	//	cout<<"mean "<<mean<<endl;
	}
	mean/=Scalars.Nrows();
			//cout<<mean<<endl;
	return mean;
}



bool meshUtils::findVertex(const Matrix & vert1,const Matrix & vert2, int ind1 ){
	for (int i=0;i<vert2.Nrows();i++){
		if ((vert1.element(ind1,0)==vert2.element(i,0)) && \
			(vert1.element(ind1,1)==vert2.element(i,1)) && \
			(vert1.element(ind1,2)==vert2.element(i,2)) ) {
			cout<<"found common point "<<" "<<endl;
			return true;
		}
	}
	
	return false;
}
void meshUtils::combinedSharedBoundaries(const string & mname1, const string & mname2 ){
	//create mesh reader
	fslvtkIO* mesh1 = new fslvtkIO;
	mesh1->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh1->readPolyData(mname1);
	
	fslvtkIO* mesh2 = new fslvtkIO;
	mesh2->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh2->readPolyData(mname2);
	
	Matrix vert1=mesh1->getPointsAsMatrix();
	Matrix vert2=mesh2->getPointsAsMatrix();
	
	Matrix Poly1=mesh1->getPolygons();
	Matrix Poly2=mesh2->getPolygons();
	vector<short> vind1,vind2;
	bool foundM12=false;
	for (int i=0; i<vert1.Nrows();i++){
		//finds the common points but need to keep common points that are connect to 2 separate vertices
		for (int j=0; j<vert2.Nrows();j++){
			if ((vert1.element(i,0)==vert2.element(j,0)) && \
				(vert1.element(i,1)==vert2.element(j,1)) && \
				(vert1.element(i,2)==vert2.element(j,2))) {
				cout<<"found common point "<<" "<<endl;
				
				for (int k=0;k<Poly1.Nrows();k++){
					if (Poly1.element(k,0)==i){
						if ( (findVertex(vert1,vert2,static_cast<int>(Poly1.element(k,1)))) || (findVertex(vert1,vert2,static_cast<int>(Poly1.element(k,2))))){
							foundM12=true;
						}
					}else if(Poly1.element(k,1)==i){
						if ( (findVertex(vert1,vert2,static_cast<int>(Poly1.element(k,0)))) || (findVertex(vert1,vert2,static_cast<int>(Poly1.element(k,2))))){
							foundM12=true;
						}
					}else if(Poly1.element(k,2)==i){
						if ( (findVertex(vert1,vert2,static_cast<int>(Poly1.element(k,0)))) || (findVertex(vert1,vert2,static_cast<int>(Poly1.element(k,1))))){
							foundM12=true;
						}
					}
				}
				if (!foundM12){
					vind1.push_back(i);
					vind2.push_back(j);
				}
			}
			
		}
	}
	
	vector<short> vindP1;
	vector<short> vindP2;
	int vindc=0;
	for ( int i=0;i<Poly1.Nrows();i++){
		for (int j=0;j<3;j++){
			if (vind1.at(vindc)==Poly1.element(i,j)){ vindP1.push_back(i); break; }
		}
		vindc++;
	}
	
	vindc=0;
	for ( int i=0;i<Poly2.Nrows();i++){
		for (int j=0;j<3;j++){
			if (vind2.at(vindc)==Poly2.element(i,j)){ vindP2.push_back(i); break; }
		}
		vindc++;
	}
	
	Matrix vert1new(vert1.Nrows()-vind1.size(),3);
	Matrix vert2new(vert2.Nrows()-vind2.size(),3);
	Matrix Poly1new(Poly1.Nrows()-vindP1.size(),3);
	Matrix Poly2new(Poly2.Nrows()-vindP2.size(),3);
	
	
	int count=0;
	vindc=0;
	for (int i=0; i<vert1.Nrows();i++){
		if (vind1.at(vindc)!=i){
			vert1new.element(count,0)=vert1.element(i,0);
			vert1new.element(count,1)=vert1.element(i,1);
			vert1new.element(count,2)=vert1.element(i,2);
		}else{
			vindc++;
		}
	}
	count=0;
	vindc=0;
	for (int i=0; i<Poly1new.Nrows();i++){
		if (vindP1.at(vindc)!=i){
			Poly1new.element(count,0)=vert1.element(i,0);
			Poly1new.element(count,1)=vert1.element(i,1);
			Poly1new.element(count,2)=vert1.element(i,2);
		}else{
			vindc++;
		}
	}
	
	count=0;
	vindc=0;
	for (int i=0; i<vert2.Nrows();i++){
		if (vind2.at(vindc)!=i){
			vert2new.element(count,0)=vert2.element(i,0);
			vert2new.element(count,1)=vert2.element(i,1);
			vert2new.element(count,2)=vert2.element(i,2);
		}else{
			vindc++;
		}
	}
	
	fslvtkIO* mout1 = new fslvtkIO;
	mout1->setDataType(static_cast<fslvtkIO::DataType>(0));
	mout1->setPoints(vert1new);
	mout1->setPolygons(vert2new);
	
	
}

void meshUtils::labelAndCombineSharedBoundaries(const string & mname1, const string & mname2, const string & mout1name ){
	//create mesh reader
	fslvtkIO* mesh1 = new fslvtkIO;
	mesh1->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh1->readPolyData(mname1);
	
	fslvtkIO* mesh2 = new fslvtkIO;
	mesh2->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh2->readPolyData(mname2);
	
	Matrix vert1=mesh1->getPointsAsMatrix();
	Matrix vert2=mesh2->getPointsAsMatrix();
	
	Matrix Poly1=mesh1->getPolygons();
	Matrix Poly2=mesh2->getPolygons();
	vector<short> vind1,vind2;
	//bool foundM12=false;
	Matrix Sc1(vert1.Nrows(),1);
	int count=0;
	for (int i=0; i<vert1.Nrows();i++){
		//finds the common points but need to keep common points that are connect to 2 separate vertices
		bool found=false;
		for (int j=0; j<vert2.Nrows();j++){
			if ((vert1.element(i,0)==vert2.element(j,0)) && \
				(vert1.element(i,1)==vert2.element(j,1)) && \
				(vert1.element(i,2)==vert2.element(j,2))) {
				cout<<"found common point "<<count<<endl;
				found=true;
				vind1.push_back(i);
				vind2.push_back(j);
				count++;
				break;
			}
		}
		if (found) { Sc1.element(i,0)=2; } else { Sc1.element(i,0)=0; }
	}
	Matrix Sc2(vert2.Nrows()-vind2.size(),1);
	Sc2=1;
	cout<<"sixes "<<vind1.size()<<" "<<vind2.size()<<endl;
	//remove vertices
	//change reference in polygonsand shift polygons matuirx
	int shift=vert1.Nrows();
	shiftPolygonMatrix(Poly2, shift);
	for (unsigned int i=0;i<vind2.size();i++){
		for ( int k=0;k<Poly2.Nrows();k++){
			for ( int l=0;l<Poly2.Ncols();l++){
				if ( Poly2.element(k,l) == vind2.at(i)+shift) {Poly2.element(k,l)=vind1.at(i); }
			}
		}
	}
	
	
	
	for (unsigned int i=0;i<vind2.size();i++){
		removeRow(vert2,vind2.at(i));
		for (unsigned int j=0; j<vind2.size();j++)
			if (vind2.at(j) > vind2.at(i)) { vind2.at(j)--; }//vind2 will store the indices of the altered vertices
				
				for ( int k=0;k<Poly2.Nrows();k++){
					for ( int l=0;l<Poly2.Ncols();l++){
						if ( Poly2.element(k,l) > (vind2.at(i)+shift)) { Poly2.element(k,l)--; }
					}
				}
				
	}
		
		/*	for (unsigned int i=0;i<vind2.size();i++){
			for ( int j=0;j<Poly2.Nrows();j++){
				for ( int k=0;k<Poly2.Ncols();k++){
					if ( Poly2.element(j,k) == vind2.at(i)+shift) { Poly2.element(j,k)=vind1.at(i); }
				}
			}
		}
*/	
		//	mesh1->setPoints(vert2);
		//	mesh1->setPolygons(Poly2);
		
		
		
		mesh1->setPoints(vert1 & vert2);
		mesh1->setPolygons(Poly1 & Poly2);
		mesh1->setScalars(Sc1 & Sc2);
		mesh1->save(mout1name);
		
}


 ReturnMatrix meshUtils::appendSharedBoundaryMask(const Matrix & Points2)
 {
	//create mesh reader
	//m1 is the template first styrcture
	//m2 is the boundary fro which we are searching for shared boundaries
	//mbase is of the same topology as m1, on which we are placing the scalars
	
	Matrix Sc1(Points.Nrows(),1);
	for (int i=0; i<Points.Nrows();i++){
		//finds the common points but need to keep common points that are connect to 2 separate vertices
		bool found=false;
		short index=-1;
		for (int j=0; j<Points2.Nrows();j++)
		{
			if ((Points.element(i,0)==Points2.element(j,0)) && (Points.element(i,1)==Points2.element(j,1)) && (Points.element(i,2)==Points2.element(j,2))) 
			{
				found=true;
				index=j;
				break;
			}
		}
		Sc1.element(i,0)=index;
	}	
	Sc1.Release();
	return Sc1;
}

void meshUtils::sampleSharedBoundaryByMask(const Matrix & Points2)
{
	for (int i=0;i<Scalars.Nrows();i++)
	{
		if (Scalars.element(i,0)>=0)
			Points.Row(i+1)=Points2.Row(static_cast<int>(Scalars.element(i,0))+1);
	}
}

/*
void meshUtils::appendSharedBoundaryMask(const string & mname1, const string & mname2,const string & mbase, const string & mout1name, const bool & indexed, const bool & useSc2 ){
	//create mesh reader
	//m1 is the template first styrcture
	//m2 is the boundary fro which we are searching for shared boundaries
	//mbase is of the same topology as m1, on which we are placing the scalars
	
	fslvtkIO* mesh1 = new fslvtkIO;
	mesh1->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh1->readPolyData(mname1);
	
	fslvtkIO* mesh2 = new fslvtkIO;
	mesh2->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh2->readPolyData(mname2);
	
	fslvtkIO* meshB = new fslvtkIO;
	meshB->setDataType(static_cast<fslvtkIO::DataType>(0));
	meshB->readPolyData(mbase);
	
	
	
	Matrix vert1=mesh1->getPointsAsMatrix();
	Matrix vert2=mesh2->getPointsAsMatrix();
	
	Matrix Poly1=mesh1->getPolygons();
	Matrix Poly2=mesh2->getPolygons();
	vector<short> vind1,vind2;
	//bool foundM12=false;
	Matrix Sc1(vert1.Nrows(),1);
	Matrix Sc2(vert2.Nrows(),1);
	Sc2=0;
	int count=0;
	for (int i=0; i<vert1.Nrows();i++){
		//finds the common points but need to keep common points that are connect to 2 separate vertices
		bool found=false;
		for (int j=0; j<vert2.Nrows();j++){
			if ((vert1.element(i,0)==vert2.element(j,0)) && \
				(vert1.element(i,1)==vert2.element(j,1)) && \
				(vert1.element(i,2)==vert2.element(j,2))) {
				cout<<"found common point "<<count<<endl;
				found=true;
				vind1.push_back(i);
				vind2.push_back(j);
				if (indexed){ count++; }
				break;
			}
		}
		if (found) { Sc1.element(i,0)=count; Sc2.element(vind2.back(),0)=count; } else { Sc1.element(i,0)=0; }
	}
	
	if (useSc2){
		meshB->setScalars(Sc2);
	}else{
		meshB->setScalars(Sc1);
	}
	
	meshB->save(mout1name);
	
	mesh2->setScalars(Sc2);
	mesh2->save("st2_"+mout1name);
	
	
}
*/
/*
void meshUtils::applyFlirtThenSBmask(const string & mname1, const string & mname2,const string & mflirtname, const string & mout1name){
	//create mesh reader
	//m1 is the template first styrcture
	//m2 is the boundary fro which we are searching for shared boundaries
	//mbase is of the same topology as m1, on which we are placing the scalars
	
	fslvtkIO* mesh1 = new fslvtkIO;
	mesh1->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh1->readPolyData(mname1);
	
	Matrix fmat=readFlirtMat(mflirtname);
	fmat=fmat.i();
	meshReg(mesh1,fmat);
	
	
	fslvtkIO* mesh2 = new fslvtkIO;
	mesh2->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh2->readPolyData(mname2);
	
	
	
	Matrix vert1=mesh1->getPointsAsMatrix();
	Matrix vert2=mesh2->getPointsAsMatrix();
	
	//	Matrix Poly1=mesh1->getPolygons();
	//	Matrix Poly2=mesh2->getPolygons();
	//	vector<short> vind1,vind2;
	//	bool foundM12=false;
	Matrix Sc1 = mesh1->getScalars();
	Matrix Sc2 = mesh2->getScalars();
	
	//	Matrix Sc2(vert2.Nrows(),1);
	//	Sc2=0;
	//int count=0;
	for (int i=0; i<vert1.Nrows();i++){
		//finds the common points but need to keep common points that are connect to 2 separate vertices
		if (Sc1.element(i,0)>0){//search for coresponding vertex index
			for (int j=0; j<Sc2.Nrows();j++){
				if (Sc2.element(j,0)==Sc1.element(i,0)){ 
					vert1.element(i,0)=vert2.element(j,0); 
					vert1.element(i,1)=vert2.element(j,1); 
					vert1.element(i,2)=vert2.element(j,2); 
					break; 
				}
			}
			
		}
	}
	
	mesh1->setPoints(vert1);
	mesh1->save(mout1name);
	
}
*/


void meshUtils::do_work_uncentreMesh(const string & inim, const string & inmesh, const string & outmesh){
	//this function is working directly on volumes
  	volume<float> im;
	read_volume(im,inim);
	float cx=(im.xsize()-1)/2.0*abs(im.xdim());
	float cy=(im.ysize()-1)/2.0*abs(im.ydim());
	float cz=(im.zsize()-1)/2.0*abs(im.zdim());
	
	
  	fslvtkIO* mesh = new fslvtkIO;
	mesh->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh->readPolyData(inmesh);
	
	Matrix MMesh = mesh->getPointsAsMatrix();
	shift3DVertexMatrix(MMesh,cx,cy,cz);
	
	mesh->setPoints(MMesh);
	mesh->save(outmesh);
	
	
	delete mesh;
	
	
	
}

void meshUtils::do_work_MeshReg(const string & inim, const string & inmesh, const string & outmesh){
	//this function is working directly on volumes
  	volume<float> im;
	read_volume(im,inim);
	float cx=(im.xsize()-1)/2.0*abs(im.xdim());
	float cy=(im.ysize()-1)/2.0*abs(im.ydim());
	float cz=(im.zsize()-1)/2.0*abs(im.zdim());
	
	
  	fslvtkIO* mesh = new fslvtkIO;
	mesh->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh->readPolyData(inmesh);
	
	Matrix MMesh = mesh->getPointsAsMatrix();
	shift3DVertexMatrix(MMesh,cx,cy,cz);
	
	mesh->setPoints(MMesh);
	mesh->save(outmesh);
	
	
	delete mesh;
	
	
	
}

 void meshUtils::meshUtils::subtractMeshes(const string & mesh1name, const string & mesh2name, const string & out){
	
  	fslvtkIO* mesh1 = new fslvtkIO;
	mesh1->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh1->readPolyData(mesh1name);
	
	fslvtkIO* mesh2 = new fslvtkIO;
	mesh2->setDataType(static_cast<fslvtkIO::DataType>(0));
	mesh2->readPolyData(mesh2name);
	
	Matrix Diff=mesh2->getPointsAsMatrix() -  mesh1->getPointsAsMatrix();
	mesh1->setVectors(Diff);
	mesh1->save(out);
	
}

void meshUtils::warpMeshWithDefField(const string & fieldname, const string & meshname, const string & meshoutname, const float & dx, const float & dy, const float & dz){
	
	fslvtkIO* ugrid = new fslvtkIO;
	ugrid->setDataType(static_cast<fslvtkIO::DataType>(0));
	ugrid->readPolyData(meshname);
	
	Matrix Points=ugrid->getPointsAsMatrix();
	//propogate volumetric mesh through deformation field //and sample intensities
	
	
	//cout<<"done flirt"<<endl;
	volume4D<float> defField;
	read_volume4D(defField, fieldname);
	
	float resx=defField[0].xdim();
	float resy=defField[0].ydim();
	float resz=defField[0].zdim();
	
	for (int i=0;i<Points.Nrows();i++){
		//shifts are for changes between roi and full image
		float px=Points.element(i,0);
		float py=Points.element(i,1);
		float pz=Points.element(i,2);
		Points.element(i,0)+=defField[0].interpolate(px/resx,py/resy,pz/resz) - dx;
		Points.element(i,1)+=defField[1].interpolate(px/resx,py/resy,pz/resz) - dy;
		Points.element(i,2)+=defField[2].interpolate(px/resx,py/resy,pz/resz) - dz;
		
	}
	
	ugrid->setPoints(Points);
	ugrid->save(meshoutname);
	
}

template< class T >
void meshUtils::warpGridWithDefField(const volume4D<T> & defField, const float & dx, const float & dy, const float & dz){
	
	float resx=defField[0].xdim();
	float resy=defField[0].ydim();
	float resz=defField[0].zdim();
	
	for (int i=0;i<Points.Nrows();i++){
		//shifts are for changes between roi and full image
		float px=Points.element(i,0);
		float py=Points.element(i,1);
		float pz=Points.element(i,2);
		
		//cout<<"points "<<px<<" "<<py<<" "<<pz<<endl;
		Points.element(i,0)=px+defField[0].interpolate(px/resx,py/resy,pz/resz) + dx;
		Points.element(i,1)=py+defField[1].interpolate(px/resx,py/resy,pz/resz) + dy;
		Points.element(i,2)=pz+defField[2].interpolate(px/resx,py/resy,pz/resz) + dz;
	}
		
}

template void meshUtils::warpGridWithDefField<float>(const volume4D<float> & fieldname, const float & dx, const float & dy, const float & dz);
template void meshUtils::warpGridWithDefField<double>(const volume4D<double> & fieldname, const float & dx, const float & dy, const float & dz);

//template<class T>
/*void meshUtils::warpMeshWithDefField( Mesh & m, const volume4D<float> & defField, const Matrix & mat){
	//mat should be 4by 4 lfirt matrix
	//cout<<"done flirt"<<endl;
	
	ColumnVector pdef(4);
	pdef.element(4)=1;
	
	float resx=defField[0].xdim();
	float resy=defField[0].ydim();
	float resz=defField[0].zdim();
	
	for (vector<mesh::Mpoint*>::iterator i=m._points.begin();i!=m._points.end();i++){
		//shifts are for changes between roi and full image
		
		
		float px=(*i)->get_coord().X;
		float py=(*i)->get_coord().Y;
		float pz=(*i)->get_coord().Z;
		pdef.element(0)=px+defField[0].interpolate(px/resx,py/resy,pz/resz);
		pdef.element(1)=py+defField[1].interpolate(px/resx,py/resy,pz/resz) ;
		pdef.element(2)=pz+defField[2].interpolate(px/resx,py/resy,pz/resz) ;
		pdef=mat*pdef;
		
		(*i)->_update_coord.X=pdef.element(0);
		(*i)->_update_coord.Y=pdef.element(1);
		(*i)->_update_coord.Z=pdef.element(2);
	}
}*/



template<class Tdist,class Tim>
void meshUtils::SurfDistToLabels( vector<Tdist> & vdist, const volume<Tim> & image){
cout<<"enter surf dist "<<endl;

		vdist.clear();
cout<<"enter surf dist "<<endl;

	int bounds[6]={0,0,0,0,0,0};
	float xdim=image.xdim();
	float ydim=image.ydim();
	float zdim=image.zdim();
	getBounds(bounds,xdim,ydim,zdim);
	//switch to using vectors later
	for (int i=0; i<Points.Nrows();i++)
	{
		cout<<"point "<<i<<endl;
		float px=Points.element(i,0);
		float py=Points.element(i,1);
		float pz=Points.element(i,2);
		int centre[3]={static_cast<short>(px), static_cast<short>(py),static_cast<short>(pz)};
		Tdist dist=10000000;
		int layer=0;
		bool notFoundD=false;
		while (notFoundD)
		{ 
			for (int x=centre[0]-layer; x<=centre[0]+layer; x++)
				for (int y=centre[1]-layer; y<=centre[1]+layer;y++)
					for (int z=centre[2]-layer; z<=centre[2]+layer;z++)
					{	
						if (image.value(x,y,z)>0)
						{
							float temp=x*x*xdim*xdim+y*y*ydim*ydim+z*z*zdim*zdim;
							if (temp<dist)
							{
								dist=temp;
								notFoundD=false;
							}
						}
					}	
						layer++;
		}
		vdist.push_back(static_cast<Tdist>(sqrt(dist)));	
	}
	
	//setScalars(vdist);
}
template void meshUtils::SurfDistToLabels<float,unsigned int>( vector<float> & dist,const volume<unsigned int>& image);
template void meshUtils::SurfDistToLabels<float,short>( vector<float> & dist,const volume<short>& image);
template void meshUtils::SurfDistToLabels<float,int>( vector<float> & dist,const volume<int>& image);
template void meshUtils::SurfDistToLabels<float,float>( vector<float> & dist,const volume<float>& image);
template void meshUtils::SurfDistToLabels<float,double>( vector<float> & dist,const volume<double>& image);

template<class Tdist,class Tim>
void meshUtils::SurfDistToLabels( vector<Tdist> & vdist, const volume<Tim> & image, const Tim & label){
		vdist.clear();


	float xdim=image.xdim();
	float ydim=image.ydim();
	float zdim=image.zdim();
	for (int i=0; i<Points.Nrows();i++)
	{
			float dist=10000000;
		cout<<"point "<<i<<endl;

		float px=Points.element(i,0);
		float py=Points.element(i,1);
		float pz=Points.element(i,2);
		for (int i=0;i<image.xsize();i++)
			for (int j=0;j<image.ysize();j++)
				for (int k=0;k<image.zsize();k++)
						{
					//		cout<<image.value(i,j,k)<<" "<<label<<endl;
							if (image.value(i,j,k)==label)
							{
							float temp=(i*xdim-px)*(i*xdim-px) + (j*ydim-py)*(j*ydim-py)+ (k*zdim-pz)*(k*zdim-pz);
							//cout<<"temp "<<temp<<endl;
							if (temp<dist)
								dist=temp;
							}
						}
		vdist.push_back(static_cast<Tdist>(sqrt(dist)));
		
	}
	
	//setScalars(vdist);
}
/*
template<class Tdist,class Tim>
void meshUtils::SurfDistToLabels( vector<Tdist> & vdist, const volume<Tim> & image, const Tim & label){
		vdist.clear();


	int bounds[6]={0,0,0,0,0,0};
	float xdim=image.xdim();
	float ydim=image.ydim();
	float zdim=image.zdim();
	getBounds(bounds,xdim,ydim,zdim);
	//switch to using vectors later
	for (int i=0; i<Points.Nrows();i++)
	{
		float px=Points.element(i,0);
		float py=Points.element(i,1);
		float pz=Points.element(i,2);
		int centre[3]={static_cast<short>(px/xdim), static_cast<short>(py/ydim),static_cast<short>(pz/zdim)};
				cout<<"centre "<<centre[0]<<" "<<centre[1]<<" "<<centre[2]<<endl;

		Tdist dist=10000000;
		int layer=0;
		bool notFoundD=true;
		cout<<"test"<<endl;
		while (notFoundD)
		{ 
			cout<<"layer "<<layer<<endl;
			for (int x=centre[0]-layer; x<=centre[0]+layer; x++)
				for (int y=centre[1]-layer; y<=centre[1]+layer;y++)
					for (int z=centre[2]-layer; z<=centre[2]+layer;z++)
					{	
					//	cout<<x<<" "<<y<<" "<<z<<" "<<label<<" "<<image.value(x,y,z)<<endl;
						if (image.value(x,y,z)==label)
						{
							float xdif=px-x*xdim;
							float ydif=py-y*ydim;
							float zdif=pz-z*zdim;
							float temp=xdif*xdif+ydif*ydif+zdif*zdif;
							cout<<"temp "<<temp<<endl;
							if (temp<dist)
							{
								dist=temp;
								notFoundD=false;
							}
						}
					}	
						layer++;
		}
		vdist.push_back(static_cast<Tdist>(sqrt(dist)));	
	}
	
	//setScalars(vdist);
}
*/
template void meshUtils::SurfDistToLabels<float,unsigned int>( vector<float> & dist,const volume<unsigned int>& image, const unsigned & label);
template void meshUtils::SurfDistToLabels<float,short>( vector<float> & dist,const volume<short>& image, const short & label);
template void meshUtils::SurfDistToLabels<float,int>( vector<float> & dist,const volume<int>& image, const int & label);
template void meshUtils::SurfDistToLabels<float,float>( vector<float> & dist,const volume<float>& image, const float & label);
template void meshUtils::SurfDistToLabels<float,double>( vector<float> & dist,const volume<double>& image, const double & label);




void meshUtils::SurfScalarsMeanAndStdev(vector<string> meshList, Matrix & MeanPoints, Matrix & MeanScalars, Matrix & StDevScalars )
{
	Matrix tempScalars;
	Matrix tempScalarsSq;

	for (unsigned int i=0;i<meshList.size();i++)
	{
		loadMesh(meshList.at(i));
		if (i==0)
		{
			MeanPoints=Points;
			tempScalars=Scalars;
			tempScalarsSq=SP(Scalars,Scalars);
		}else
		{
			MeanPoints+=Points;
			tempScalars+=Scalars;
			tempScalarsSq+=SP(Scalars,Scalars);
		}
	}
	float N=static_cast<float>(meshList.size());
	MeanPoints/=N;
	MeanScalars=tempScalars/N;
	StDevScalars=(SP(tempScalars,tempScalars)-tempScalarsSq/N)/(N-1);
	for (int i=0; i<StDevScalars.Nrows();i++)
		StDevScalars.element(i,0)=sqrt(StDevScalars.element(i,0));
	
}


template<class T>
void meshUtils::warpMeshWithDefField(mesh::Mesh & m, const volume4D<T> & defField, const Matrix & mat){
			//mat should be 4by 4 lfirt matrix
	//cout<<"done flirt"<<endl;
	
	ColumnVector pdef(4);
	pdef.element(3)=1;
	
	float resx=defField[0].xdim();
	float resy=defField[0].ydim();
	float resz=defField[0].zdim();
	
	for (vector<mesh::Mpoint*>::iterator i=m._points.begin();i!=m._points.end();i++){
		//shifts are for changes between roi and full image
		
		
		float px=(*i)->get_coord().X;
		float py=(*i)->get_coord().Y;
		float pz=(*i)->get_coord().Z;
		pdef.element(0)=px+defField[0].interpolate(px/resx,py/resy,pz/resz);
		pdef.element(1)=py+defField[1].interpolate(px/resx,py/resy,pz/resz) ;
		pdef.element(2)=pz+defField[2].interpolate(px/resx,py/resy,pz/resz) ;
		pdef=mat*pdef;
		
		(*i)->_update_coord.X=pdef.element(0);
		(*i)->_update_coord.Y=pdef.element(1);
		(*i)->_update_coord.Z=pdef.element(2);
	}
	m.update();
}
template void meshUtils::warpMeshWithDefField<float>( Mesh & m, const volume4D<float> & defField, const Matrix & mat);
template void meshUtils::warpMeshWithDefField<int>( Mesh & m, const volume4D<int> & defField, const Matrix & mat);
template void meshUtils::warpMeshWithDefField<double>( Mesh & m, const volume4D<double> & defField, const Matrix & mat);
template void meshUtils::warpMeshWithDefField<short>( Mesh & m, const volume4D<short> & defField, const Matrix & mat);


//template void meshUtils::warpMeshWithDefField<float>( Mesh & m, const volume4D<float> & defField, const Matrix & mat);




template<class T>
void meshUtils::ugridToImage(volume<T> & im)
{
	//for now use nearest neighbour
	float xres=im.xdim();
	float yres=im.ydim();
	float zres=im.zdim();
	cout<<"scalars size "<<Scalars.Nrows()<<" "<<xres<<" "<<yres<<" "<<zres<<endl;
	//float dist_th=sqrt(xres+xres+yres*yres+zres*zres);
	im=0;
	for (int i=0;i<Points.Nrows();i++)
	{
       	//cout<<"i "<<i<<endl;
	  im.value(static_cast<int>(Points.element(i,0)/xres),static_cast<int>(Points.element(i,1)/yres),static_cast<int>(Points.element(i,2)/zres))=static_cast<T>(Scalars.element(i,0));
		//cout<<Scalars.element(i,0)<<" "<<Scalars.Nrows()<<endl;
	}
	
	
	/*
	for (int i=0;i<im.xsize();i++)
	{
		cout<<"iter "<<i<<" of "<<im.xsize()<<endl;
		for (int j=0;j<im.ysize();j++)
		{
			for (int k=0;k<im.zsize();k++)
			{
				//search for closest point 
				float mindist=(Points.element(0,0)-i*xres)*(Points.element(0,0)-i*xres) \
				+(Points.element(0,1)-j*yres)*(Points.element(0,1)-j*yres) \
				+(Points.element(0,2)-k*zres)*(Points.element(0,2)-k*zres) ;
				int indmin=0;
				for (int x=1;x<Points.Nrows();x++)
				{
					float dx=(Points.element(x,0)-i*xres);
					float dy=(Points.element(x,1)-j*yres);
					float dz=(Points.element(x,2)-k*zres);
					
					if ((dx*dx+dy*dy+dz*dz) < mindist){
						//				cout<<i<<" "<<j<<" "<<k<<" search "<<indmin<<" "<<Points.element(x,0)<<" "<<Points.element(x,1)<<" "<<Points.element(x,2)<<" "<<dx<<" "<<dy<<" "<<dz<<endl;
						indmin=x;
						mindist=(dx*dx+dy*dy+dz*dz); 
						if (mindist<dist_th) break;
					}
				}	
				cout<<"indmind "<<indmin<<" "<<mindist<<" "<<dist_th<<endl;
				im.value(i,j,k)=static_cast<T>(Scalars.element(indmin,0));
		
			}
			//	cout<<i<<" "<<j<<" "<<k<<" INDMIN "<<indmin<<" "<<Points.element(indmin,0)<<" "<<Points.element(indmin,1)<<" "<<Points.element(indmin,2)<<endl;
				
		}
	}
	*/
	
}	
template void meshUtils::ugridToImage<short>(volume<short> & im);
template void meshUtils::ugridToImage<float>(volume<float> & im);
template void meshUtils::ugridToImage<double>(volume<double> & im);


ReturnMatrix meshUtils::getDeformedVector(const ColumnVector & mean, const Matrix & modes, const ColumnVector & eigs, const vector<float> & vars )
{
	ColumnVector def(mean.Nrows());
	def=mean;
	for (unsigned int i=0;i<vars.size();i++)
	{
		def+=modes.Column(i+1)*sqrt(eigs.element(i))*vars.at(i);
		//cout<<"var1 "<<vars.at(i)<<" "<<sqrt(eigs.element(i))<<endl;
	}	
		
	def.Release();
	return def;

}

template<class T,class T2>
void meshUtils::deformSurface(const volume<T> & image, const float & maxit, const float & wIm, const float & wTang_in, const float & w_area, const float & w_norm, const T & max_thresh, const string & name)
{
	const T  max_thresh_sq=max_thresh*max_thresh;
		const T xdim = static_cast<T>(image.xdim()); 
		const T ydim = static_cast<T>(image.ydim()); 
		const T zdim = static_cast<T>(image.zdim()); 
	float wTang=wTang_in;

#ifdef USE_VTK
	
	vtkPolyData *meshData = vtkPolyData::New();
	vtkPolyDataReader *vtkmesh = vtkPolyDataReader::New();
	vtkRenderWindow *renWin = vtkRenderWindow::New();
	vtkRenderer *ren1 =vtkRenderer::New();
	vtkPolyDataMapper *meshmap = vtkPolyDataMapper::New();
	vtkRenderWindowInteractor *iren = vtkRenderWindowInteractor::New();
	vtkImageData *imStruct =vtkImageData::New();
	vtkImagePlaneWidget *planeWidgetX = vtkImagePlaneWidget::New();
	
	vtkmesh->SetFileName(name.c_str());
	meshData=(vtkmesh->GetOutput());
	meshmap->SetInput(meshData);
	
	//actor coordinates, geome..etc
	vtkActor *aSurf = vtkActor::New();
	aSurf->GetProperty()->SetColor(0.7,0,0);
	aSurf->GetProperty()->SetOpacity(0.95); 
	aSurf->GetProperty()->SetEdgeColor(0.0,0,8);
	aSurf->GetProperty()->SetEdgeVisibility(1);
	aSurf->GetProperty()->SetPointSize(2);
	aSurf->GetProperty()->SetRepresentationToWireframe();
	aSurf->SetMapper(meshmap);
	
	//interactorv
	iren->SetRenderWindow(renWin);
	
	//add actor
	ren1->AddActor(aSurf);
	//ren1->AddActor(aSphere2);
	ren1->SetBackground(0.4,0.4,0.4);
	
	//Render
	renWin->AddRenderer(ren1);
	
	
//	imStruct=newimageTovtkImageData(image);	

  	
//  planeWidgetX->DisplayTextOn();
  //planeWidgetX->SetInput(imStruct);
//  planeWidgetX->SetPlaneOrientationToYAxes();
//  planeWidgetX->SetSliceIndex(120);
//  planeWidgetX->SetKeyPressActivationValue('x');
//  planeWidgetX->SetInteractor(iren);
//  planeWidgetX->TextureVisibilityOn();
//  planeWidgetX->GetTexturePlaneProperty()->SetOpacity(0.9);
  
 	renWin->Render();
//	  iren->Start();


#endif
  
	vector<T> pts_org=getPointsAsVector<T>();
	vector<T> pts=pts_org;
	
	
	vector<  vector<T2> > cells=this->getPolygonsAsVectorOfVectors<T2>();
	vector<  vector<T2> > neighbours=first_mesh::findNeighbours<T2>(cells, pts.size()/3);
	vector<  vector<T2> > neighbour_tris=first_mesh::findNeighbourTriangles<T2>(cells, pts.size()/3);

	
	
	for (int n_iter=1; n_iter<=maxit; n_iter++) 
	{  //iterations
	//	cout<<"n "<<n_iter<<endl;
		
		//calculate normals
		vector<T> vnx,vny,vnz;
		first_mesh::normal<T,T2>(pts,neighbour_tris,cells,vnx,vny,vnz);
		
		vector<T> vtri_x,vtri_y,vtri_z;
		first_mesh::maxTriangle<T,T2>(pts,neighbour_tris,cells,vtri_x,vtri_y,vtri_z);

		//	first_mesh::normal<T>(pts,triangles,vnx,vny,vnz);
	
	//	cout<<"normals"<<endl;		
//				typename vector<T>::iterator a=vny.begin();		
//		typename vector<T>::iterator b=vnz.begin();
//				typename vector<T>::iterator d=pts.begin();		
//		for (typename vector<T>::iterator c=vnx.begin();c!=vnx.end();c++,a++,b++,d+=3)
//		{
//			cout<<"vertex "<<*d<<" "<<*(d+1)<<" "<<*(d+2)<<endl;
//			cout<<"local normal "<<*c<<" "<<*a<<" "<<*b<<endl;
//		}
		//clauclate medium of neigbours than teh diference
		vector<T> vmx,vmy,vmz;
		first_mesh::medium_neighbours<T,T2>(pts,neighbours,cells,vmx,vmy,vmz);
		
	
		typename vector<T>::iterator j=vmy.begin();
		typename vector<T>::iterator k=vmz.begin();
		typename vector<T>::iterator l=pts.begin();
		
		typename vector<T>::iterator m=vnx.begin();
		typename vector<T>::iterator n=vny.begin();		
		typename vector<T>::iterator o=vnz.begin();
		//find diffenrece vector
		
			vector<T> v_im_nx,v_im_ny,v_im_nz;
		for (typename vector<T>::iterator i=vmx.begin();i!=vmx.end();i++,j++,k++,m++,n++,o++,l+=3)
		{
			//calculate difference vector
			*i-=*l;
			*j-=*(l+1);
			*k-=*(l+2);
	//					cout<<"diff vec "<<(*i)<<" "<<(*j)<<" "<<(*k)<<endl;

			T im_int = image.interpolate( (*l)/xdim, (*(l+1))/ydim,  (*(l+2))/zdim );
			v_im_nx.push_back(im_int * (*m) );
			v_im_ny.push_back(im_int * (*n) );
			v_im_nz.push_back(im_int * (*o) );

			
			T norm= (*i) * (*m)+ (*j) * (*n) + (*k) * (*o);
		//	cout<<"norm "<<norm<<endl;
			(*m) *= norm;
			(*n) *= norm;
			(*o) *= norm;
			
			(*i)-=(*m);
			(*j)-=(*n);
			(*k)-=(*o);
			
			
		}
		
	//	cout<<"medium"<<endl;
		
		vector<T> v_dx, v_dy, v_dz;
		//		float maxstep=0;
		typename vector<T>::iterator tang_xi = vmx.begin();
		typename vector<T>::iterator tang_yi = vmy.begin();
		typename vector<T>::iterator tang_zi = vmz.begin();
		typename vector<T>::iterator norm_xi = vnx.begin();
		typename vector<T>::iterator norm_yi = vny.begin();
		typename vector<T>::iterator norm_zi = vnz.begin();
		typename vector<T>::iterator norm_im_xi = v_im_nx.begin();
		typename vector<T>::iterator norm_im_yi = v_im_ny.begin();
		typename vector<T>::iterator norm_im_zi = v_im_nz.begin();
		typename vector<T>::iterator vtri_xi = vtri_x.begin();
		typename vector<T>::iterator vtri_yi = vtri_y.begin();
		typename vector<T>::iterator vtri_zi = vtri_z.begin();
		
	//	cout<<"delats"<<endl;
		float max_step=0;
		for (typename vector<T>::iterator i=pts.begin();i!=pts.end();i+=3, tang_xi++,tang_yi++,tang_zi++,norm_xi++,norm_yi++,norm_zi++, norm_im_xi++ , norm_im_yi++ , norm_im_zi++, vtri_xi++, vtri_yi++, vtri_zi++ )
		{
			
			//cout<<wTang<<" "<<w_norm<<" "<<xdim<<" "<<ydim<<" "<<zdim<<" "<<(*i)/xdim<<" "<<(*(i+1))/ydim<<" "<<(*(i+2))/zdim<<" "<<image.interpolate( (*i)/xdim , (*(i+1))/ydim,  (*(i+2))/zdim )<<endl;
			float dx = wTang * (*tang_xi) + w_norm * (*norm_xi) + wIm * (*norm_im_xi) + w_area * (*vtri_xi);
			float dy = wTang * (*tang_yi) + w_norm * (*norm_yi) + wIm * (*norm_im_yi) + w_area * (*vtri_yi);
			float dz = wTang * (*tang_zi) + w_norm * (*norm_zi) + wIm * (*norm_im_zi) + w_area * (*vtri_zi);
			v_dx.push_back( dx );//+ wIm ;//* image.interpolate( 1, 1,1);// (*(i+1))/ydim,  (*(i+2))/zdim );
				v_dy.push_back( dy );
				v_dz.push_back( dx );
			//	cout<<"updist "<<dx<<" "<<dy<<" "<<dz<<" "<<(dx*dx +dy*dy +dz*dz)<<" "<<(*norm_xi)<<" "<<(*norm_yi)<<" "<<(*norm_zi)<<endl;
				
				//max_thresh_sq=max_thresh*max_thresh
				if ( (dx*dx +dy*dy +dz*dz) > max_step )
					max_step=(dx*dx +dy*dy +dz*dz);
				
				//	v_dx.push_back(wIm * fim_xi + wTang * tang_xi + maxTri * maxtri_xi + w_norm * norm_xi);
				//	v_dy.push_back(wIm * fim_yi + wTang * tang_yi + maxTri * maxtri_yi + w_norm * norm_yi);
				//	v_dz.push_back(wIm * fim_zi + wTang * tang_zi + maxTri * maxtri_zi + w_norm * norm_zi);
				//			float maxstep_temp=v_dx.back()*v_dx.back() + v_dy.back()*v_dy.back() + v_dz.back()*v_dz.back();
				//fi (maxstep>
	}
	
	float scale=1;
//	cout<<"max_step "<<max_step<<" "<<max_thresh<<endl;
		if ( max_step > max_thresh_sq)
			scale=max_thresh/sqrt(max_step);
//		cout<<"scale "<<scale<<endl;
		//update points
		typename vector<T>::iterator dx_i = v_dx.begin();
		typename vector<T>::iterator dy_i = v_dy.begin();
		typename vector<T>::iterator dz_i = v_dz.begin();
	//	int count=0;
		for (typename vector<T>::iterator i=pts.begin();i!=pts.end();i+=3,dx_i++,dy_i++,dz_i++)
		{
			//	cout<<"count "<<count<<" "<<*i<<" "<<*(i+1)<<" "<<*(i+2)<<endl;
			
			*i+=(*dx_i)*scale;
			*(i+1)+=(*dy_i)*scale;
			*(i+2)+=(*dz_i)*scale;
	//		count++;
		//	cout<<"count "<<count<<endl;
		}
	
		if ( n_iter % 100 == 0)
			if (first_mesh::self_intersection_test<T,T2>(cells,pts))
			{
			pts=pts_org;
			wTang+=0.1;
			cout<<"THE MESH SELF-INTERSECTS "<<n_iter<<" "<<wTang<<endl;
			n_iter=0;
		}
//		volume<float> fslim;
//	read_volume(fslim,"ststs");	
//	cout<<"convert to vtk"<<endl;
//	image=newimageTovtkImageData(fslim);
		
#ifdef USE_VTK
		fslvtkconvname::update_vtkPolyData(pts,meshData);
		meshData->GetPoints()->Modified();
	//	cout<<"render"<<endl;
//	for (int i=0;i<100000000;i++)
//		int a=1;
		
		renWin->Render();
#endif
}
}

template void meshUtils::deformSurface<float,short>(const volume<float> & image, const float & maxit, const float & wIm, const float & wTang, const float & maxTri, const float & w_norm, const float & max_thresh, const string & name);
template void meshUtils::deformSurface<float,unsigned int>(const volume<float> & image, const float & maxit, const float & wIm, const float & wTang, const float & maxTri, const float & w_norm, const float & max_thresh, const string & name);
//template void meshUtils::deformSurface<double,unsigned int>(const volume<float> & image, const float & maxit, const float & wIm, const float & wTang, const float & maxTri, const float & w_norm, const float & max_thresh, const string & name);

//template void meshUtils::deformSurface<double,short>(const float & maxit, const float & wIm, const float & wTang, const float & maxTri, const float & w_norm, const string & name);
//template void meshUtils::deformSurface<double,unsigned int>(const float & maxit, const float & wIm, const float & wTang, const float & maxTri, const float & w_norm, const string & name);


void meshUtils::applyReg(Matrix & pts, const Matrix & fmat)
{
	ColumnVector ones(pts.Nrows());
	ones=1;
	Matrix temp= (pts | ones);
//	cout<<"dikms "<<Pts_src_dm.Nrows()<<" "<<Pts_src_dm.Ncols()<<" "<<ones.Nrows()<<" "<<ones.Ncols()<<endl;//temp.Ncols()<<endl;
	temp=(fmat*(pts | ones).t());
	temp=temp.t();
	pts=temp.SubMatrix(1,temp.Nrows(),1,3);

}

ReturnMatrix meshUtils::calculateRotation(const Matrix & Pts_src_dm, const Matrix & Pts_targ_dm)
{	
	Matrix M_Rot(4,4);
	M_Rot=0;
	M_Rot.element(3,3)=1;


		Matrix M=Pts_targ_dm.t()*(Pts_src_dm);
		Matrix U;
		DiagonalMatrix D;
		SVD(M.t()*M,D,U);
		//M should always be a 3x3 matrix
		for (int i=0;i<D.Nrows();i++){
			D.element(i)=1/sqrt(D.element(i));
		}
		
		Matrix R(3,3);
		R=M*(U*D*U.t());
		
		
		SVD(R,D,U);
//cout<<"D "<<D<<endl;
		
		M_Rot.SubMatrix(1,3,1,3)=R;
	
		M_Rot.Release();
		return M_Rot;
}
ReturnMatrix meshUtils::calculateScale(const Matrix & Pts_src_dm, const Matrix & Pts_targ_dm, const bool & global)
{	
	Matrix M_Sc(4,4);
	M_Sc=0;
	M_Sc.element(0,0)=1;M_Sc.element(1,1)=1;M_Sc.element(2,2)=1;M_Sc.element(3,3)=1;
		float sumrx=0, sumry=0,sumrz=0, sumlx=0,sumly=0, sumlz=0;
			for (unsigned int i=0;i<static_cast<unsigned int>(Pts_src_dm.Nrows()) ;i++){
				sumlx += Pts_src_dm.element(i,0)*Pts_src_dm.element(i,0);
				sumly += Pts_src_dm.element(i,1)*Pts_src_dm.element(i,1);
			    sumlz += Pts_src_dm.element(i,2)*Pts_src_dm.element(i,2);
			  
				sumrx += Pts_targ_dm.element(i,0)*Pts_targ_dm.element(i,0);
				sumry += Pts_targ_dm.element(i,1)*Pts_targ_dm.element(i,1);
				sumrz += Pts_targ_dm.element(i,2)*Pts_targ_dm.element(i,2);
			
			}
			if (global)
			{
				float scale=sqrt((sumrx+sumry+sumrz)/(sumlx+sumly+sumlz));
				M_Sc.element(0,0)=scale;M_Sc.element(1,1)=scale;M_Sc.element(2,2)=scale;
			}else {
								M_Sc.element(0,0)=sqrt(sumrx/sumlx);M_Sc.element(1,1)=sqrt(sumry/sumly);M_Sc.element(2,2)=sqrt(sumrz/sumlz);
			}
			M_Sc.Release();
			return M_Sc;
}

Matrix meshUtils::reg_leastsq(const Matrix & SourcePoints, const short & dof)
{
	
	unsigned int Npoints=Points.Nrows(); 
	
		Matrix M_trans_src(4,4);
	M_trans_src=0;
	M_trans_src.element(0,0)=1;	M_trans_src.element(1,1)=1;	M_trans_src.element(2,2)=1;M_trans_src.element(3,3)=1;

	
	Matrix M_trans_targ(4,4);
	M_trans_targ=0;
	M_trans_targ.element(0,0)=1;	M_trans_targ.element(1,1)=1;	M_trans_targ.element(2,2)=1;M_trans_targ.element(3,3)=1;
	
	
	Matrix M_Sc(4,4);
	M_Sc=0;
	M_Sc.element(0,0)=1;M_Sc.element(1,1)=1;M_Sc.element(2,2)=1;M_Sc.element(3,3)=1;
	
	Matrix M_Rot(4,4);
	M_Rot=0;
	M_Rot.element(0,0)=1;M_Rot.element(1,1)=1;M_Rot.element(2,2)=1;M_Rot.element(3,3)=1;
	//---------------------CALCULATE CENTROIDS----------------------//
	double mean_src_x=0, mean_src_y=0, mean_src_z=0;
	double mean_targ_x=0, mean_targ_y=0,mean_targ_z=0;
	
	for (unsigned int i=0; i<Npoints;i++)
	{
		mean_src_x+=SourcePoints.element(i,0);
		mean_src_y+=SourcePoints.element(i,1);
		mean_src_z+=SourcePoints.element(i,2);

		mean_targ_x+=Points.element(i,0);
		mean_targ_y+=Points.element(i,1);
		mean_targ_z+=Points.element(i,2);
	} 
		
		mean_src_x/=Npoints;
		mean_src_y/=Npoints;
		mean_src_z/=Npoints;

		mean_targ_x/=Npoints;
		mean_targ_y/=Npoints;
		mean_targ_z/=Npoints;

	//---------------------TRANSLATION COMPONENTS----------------------//

	M_trans_src.element(0,3)=-mean_src_x;	M_trans_src.element(1,3)=-mean_src_y;	M_trans_src.element(2,3)=-mean_src_z;
	
	M_trans_targ.element(0,3)=mean_targ_x;	M_trans_targ.element(1,3)=mean_targ_y;	M_trans_targ.element(2,3)=mean_targ_z;
	
	//---------------------DEMEAN POINTS ---------------------------//

	Matrix Pts_src_dm=SourcePoints;
	Matrix Pts_targ_dm=Points;
	
	for (unsigned int i=0;i<Npoints;i++)
	{
		 Pts_src_dm.element(i,0)-=mean_src_x;
		 Pts_src_dm.element(i,1)-=mean_src_y;
		 Pts_src_dm.element(i,2)-=mean_src_z;
		 
		 Pts_targ_dm.element(i,0)-=mean_targ_x;
		 Pts_targ_dm.element(i,1)-=mean_targ_y;
		 Pts_targ_dm.element(i,2)-=mean_targ_z;
	}

	
	
		//---------------------ROTATION COMPONENT (OPTIONAL)----------------------//
Matrix M_Sc_final=M_Sc;
Matrix M_Rot_final=M_Rot;

for (unsigned int i=0;i<100;i++)
{ 
	if (dof>=6)
	{
		M_Rot=calculateRotation(Pts_src_dm,Pts_targ_dm);
		M_Rot_final=M_Rot*M_Rot_final;
		if (dof==6)
			break;
	}//----------------------ADD NINE DEGREES OF FREEDON---------------------//
		//---------------------SCALE COMPONENT (OPTIONAL)----------------------//
//	cout<<"do Scale"<<endl;
	if (dof==7)
	{
		M_Sc_final=calculateScale(Pts_src_dm,Pts_targ_dm,true);
		break;
	}else if (dof==9)
	{
		applyReg(Pts_src_dm,M_Rot);
		M_Sc=calculateScale(Pts_src_dm,Pts_targ_dm,false);
		M_Sc_final=M_Sc*M_Sc_final;
		applyReg(Pts_src_dm,M_Sc);

	}
//	cout<<"rot "<<M_Rot<<endl;
//	cout<<"sc "<<M_Sc<<endl;
	}
///	cout<<"trans src "<<M_trans_src<<endl;
//		cout<<"trans targ "<<M_trans_targ<<endl;

	cout<<"rot "<<M_Rot_final<<endl;
	cout<<"sc "<<M_Sc_final<<endl;
//	cout<<M_trans_targ  *  M_Sc_final * M_Rot_final * M_trans_src<<endl;
	
	return M_trans_targ * M_Sc_final * M_Rot_final * M_trans_src;
	
}

Matrix meshUtils::alignSurfaces(const string & src_list, const short & dof, const string & outname )
{

	Matrix allPoints(1,1);
	ifstream innames;
	innames.open(src_list.c_str());
	string fname;

	fslvtkIO* fout = new fslvtkIO();
	while (innames>>fname)
	{
		meshUtils* meshIn=new meshUtils(fname,static_cast<meshUtils::DataType>(0));
		//Matrix Pts_src=;
		Matrix fmat=this->reg_leastsq(meshIn->getPointsAsMatrix(),dof);
		//cout<<"points orh "<<meshIn->getPointsAsMatrix()<<endl;
				meshIn->meshReg(fmat);
		if ((allPoints.Nrows()==1)&&(allPoints.Ncols()==1))
		{
			if (strcmp(outname.c_str(),"nosave"))
			{
				cout<<"adding DDDDDDDpoints"<<endl;
			fout->setPolygons(meshIn->getPolygons());
			fout->setPoints(meshIn->getPointsAsMatrix());						
			}
			allPoints = first_newmat_vector::unwrapMatrix(meshIn->getPointsAsMatrix());
		}else 
		{
			cout<<fout->getPointsAsMatrix().Nrows()<<" "<<fout->getPointsAsMatrix().Ncols()<<" "<<meshIn->getPointsAsMatrix().Nrows()<<" "<<meshIn->getPointsAsMatrix().Ncols()<<" "<<endl;
			if (strcmp(outname.c_str(),"nosave"))
					fout->appendPointsAndPolygons(meshIn->getPointsAsMatrix(), meshIn->getPolygons());						
			allPoints = allPoints |   first_newmat_vector::unwrapMatrix(meshIn->getPointsAsMatrix());
		}
			//	cout<<"points reg "<<meshIn->getPointsAsMatrix()<<endl;

		//meshIn->save(outname.value());
		cout<<"allPoints "<<allPoints.Nrows()<<" "<<allPoints.Ncols()<<endl;
		delete meshIn;
	}
	innames.close();

			if (strcmp(outname.c_str(),"nosave"))
			fout->save(outname);
	
	delete fout;
	
	return allPoints;
}



}	
