float PropagatedDeformableModel::computeContrast(Referential& refInitial)
{
	// Calcul du profil de la moelle et du LCR perpendiculairement au tube
	CVector3 pointS, indexS;
	vector<float> contrast(resolutionRadiale_);
	float angle;
	CMatrix3x3 trZ;
	CMatrix4x4 transformationFromOrigin = refInitial.getTransformationInverse();
	float factor = image3D_->getTypeImageFactor();
	for (int k=0; k<resolutionRadiale_; k++)
	{
		vector<float> profilIntensite;
		angle = 2*M_PI*k/(double)resolutionRadiale_;
		trZ[0] = cos(angle), trZ[1] = sin(angle), trZ[3] = -sin(angle), trZ[4] = cos(angle);
		for (int l=0; l<2.5*rayon_; l++) {
			pointS = transformationFromOrigin*(trZ*CVector3(l,0.0,0.0));
			if (image3D_->TransformPhysicalPointToIndex(pointS,indexS))
				profilIntensite.push_back(factor*image3D_->GetPixelOriginal(indexS));
		}
		float min = 0.0, max = 0.0, maxVal = 0.0, valCourante;
		unsigned int m = 0;
		for (unsigned int i=1; i<profilIntensite.size(); i++) {
			valCourante = profilIntensite[i]-profilIntensite[i-1];
			if (maxVal <= valCourante) {
				maxVal = valCourante;
				m = i;
			}
		}
		if (profilIntensite.size() > 0)
		{
			min = profilIntensite[m];
			for (unsigned int j=0; j<m; j++) {
				valCourante = profilIntensite[j];
				if (min > valCourante) min = valCourante;
			}
			max = profilIntensite[m];
			for (unsigned int j=m+1; j<profilIntensite.size(); j++) {
				valCourante = profilIntensite[j];
				if (max < valCourante) max = valCourante;
			}
		}
		contrast[k] = abs(max-min);
	}
	float result = 0.0;
	for (unsigned int i=0; i<contrast.size(); i++)
		result += contrast[i];
	result /= contrast.size();

	return result;
}
void PropagatedDeformableModel::computeNewBand(SpinalCord* mesh, CVector3 initialPoint, CVector3 nextPoint, int resolution)
{
	CMatrix4x4 transformationFromOrigin;
	double angle;
	CMatrix3x3 trZ;
	CVector3 point, normale;
	CVector3 directionCourante = nextPoint-initialPoint, lastNormal = (nextPoint-initialPoint).Normalize(), directionCourantePerpendiculaire;
	if (lastNormal[2] == 0.0) directionCourantePerpendiculaire = CVector3(0.0,0.0,1.0);
	else directionCourantePerpendiculaire = CVector3(1.0,2.0,-(lastNormal[0]+2*lastNormal[1])/lastNormal[2]).Normalize();

    CVector3 stretchingFactorWorld = image3D_->TransformContinuousIndexToPhysicalPoint(CVector3((1-1.0/stretchingFactor_), 0.0, 0.0))-image3D_->getOrigine();
    
	int offsetTriangles = mesh->getNbrOfPoints()-resolutionRadiale_;
	for (int len=1; len<=resolution; len++)
	{
		CVector3 pointIntermediaire = initialPoint + len*directionCourante/(double)resolutionAxiale_;
		Referential refCourant = Referential(lastNormal^directionCourantePerpendiculaire, directionCourantePerpendiculaire, lastNormal, pointIntermediaire);
		transformationFromOrigin = refCourant.getTransformationInverse();
		for (int k=0; k<resolutionRadiale_; k++)
		{
			angle = 2*M_PI*k/(double)resolutionRadiale_;
			trZ[0] = cos(angle), trZ[1] = sin(angle), trZ[3] = -sin(angle), trZ[4] = cos(angle);
			point = transformationFromOrigin*(trZ*CVector3(rayon_,0.0,0.0));
            CVector3 vecPoint = initialPoint - point;
            point[0] += stretchingFactorWorld[0]*vecPoint[0];
            point[1] += stretchingFactorWorld[1]*vecPoint[1];
            point[2] += stretchingFactorWorld[2]*vecPoint[2];
			mesh->addPoint(new Vertex(point,(point-pointIntermediaire).Normalize()));
		}
		// Ajout des triangles - attention � la structure en cercle
		for (int k=0; k<resolutionRadiale_-1; k++)
		{
			mesh->addTriangle(offsetTriangles+(len-1)*resolutionRadiale_+k,offsetTriangles+(len-1)*resolutionRadiale_+k+1,offsetTriangles+len*resolutionRadiale_+k);
			mesh->addTriangle(offsetTriangles+(len-1)*resolutionRadiale_+k+1,offsetTriangles+len*resolutionRadiale_+k+1,offsetTriangles+len*resolutionRadiale_+k);
		}
		// Ajout des deux derniers triangles pour fermer le tube
		mesh->addTriangle(offsetTriangles+(len-1)*resolutionRadiale_+resolutionRadiale_-1,offsetTriangles+(len-1)*resolutionRadiale_,offsetTriangles+len*resolutionRadiale_+resolutionRadiale_-1);
		mesh->addTriangle(offsetTriangles+(len-1)*resolutionRadiale_,offsetTriangles+len*resolutionRadiale_,offsetTriangles+len*resolutionRadiale_+resolutionRadiale_-1);
	}
}
void PropagatedDeformableModel::computeMeshInitial()
{
    // centerline can be added to be followed. Points of centerline have to be added from bottom to top
    if (propCenterline_) {
        if (centerline.size() < 1) {
            cerr << "Error: Not enought points in centerline" << endl;
            return;
        } else {
            //int init = centerline.size()*init_position_;
            
            /******************************************************************************************
             * If a centerline is used for the orientation computation, we compute its BSpline approximation. This approximation is used for centerline position and orientation extraction
             * The parameter range is the centerline approximation accuracy
             *****************************************************************************************/
            initial_centerline = centerline;
            centerline_approximator = BSplineApproximation(&initial_centerline);
            
            initialPoint_ = centerline_approximator.EvaluateBSpline(init_position_);
            initialNormal1_ = -centerline_approximator.EvaluateGradient(init_position_-0.01).Normalize();
            initialNormal2_ = centerline_approximator.EvaluateGradient(init_position_+0.01).Normalize();;
            //initialNormal1_ = (centerline[init-1]-centerline[init]).Normalize();
            //initialNormal2_ = (centerline[init+1]-centerline[init]).Normalize();
            hasInitialPointAndNormals_ = true;
        }
    }
	if (centerline.size() < 1 && !hasInitialPointAndNormals_) {
		cerr << "Error: Not enought points in centerline" << endl;
	}
	else if (centerline.size() == 2) {
		initialTube1 = new SpinalCord;
		CVector3 directionInitiale = (centerline[1]-centerline[0]).Normalize(), directionInitialePerpendiculaire;
		if (directionInitiale[2] == 0.0) directionInitialePerpendiculaire = CVector3(0.0,0.0,1.0);
		else directionInitialePerpendiculaire = CVector3(1.0,2.0,-(directionInitiale[0]+2*directionInitiale[1])/directionInitiale[2]).Normalize();
		Referential refInitial = Referential(directionInitiale^directionInitialePerpendiculaire, directionInitialePerpendiculaire, directionInitiale, centerline[0]);
		CMatrix4x4 transformationFromOrigin = refInitial.getTransformationInverse();
		double angle;
		CMatrix3x3 trZ;
		CVector3 point, normale;
		// Compute initial disk
		for (int k=0; k<resolutionRadiale_; k++)
		{
			angle = 2*M_PI*k/(double)resolutionRadiale_;
			trZ[0] = cos(angle), trZ[1] = sin(angle), trZ[3] = -sin(angle), trZ[4] = cos(angle);
			point = transformationFromOrigin*(trZ*CVector3(rayon_,0.0,0.0));
			initialTube1->addPoint(new Vertex(point,(point-centerline[0]).Normalize()));
		}
		CVector3 nextPoint = centerline[0] + deplacementAxial_*directionInitiale;
		computeNewBand(initialTube1,centerline[0],nextPoint,resolutionAxiale_+2);

		initialTube1->computeConnectivity();
		initialTube1->computeTrianglesBarycentre();

		isMeshInitialized = true;

		meanContrast = computeContrast(refInitial);
		//cout << "Contrast = " << contrast << endl;
	}
	else if (hasInitialPointAndNormals_) {
		initialTube1 = new SpinalCord;
		CVector3 directionInitiale = initialNormal1_, directionInitialePerpendiculaire;
		if (directionInitiale[2] == 0.0) directionInitialePerpendiculaire = CVector3(0.0,0.0,1.0);
		else directionInitialePerpendiculaire = CVector3(1.0,2.0,-(directionInitiale[0]+2*directionInitiale[1])/directionInitiale[2]).Normalize();
		Referential refInitial = Referential(directionInitiale^directionInitialePerpendiculaire, directionInitialePerpendiculaire, directionInitiale, initialPoint_);
		CMatrix4x4 transformationFromOrigin = refInitial.getTransformationInverse();
		double angle;
		CMatrix3x3 trZ;
		CVector3 point, normale;
        CVector3 stretchingFactorWorld = image3D_->TransformContinuousIndexToPhysicalPoint(CVector3((1.0-1.0/stretchingFactor_), 0.0, 0.0))-image3D_->getOrigine();
		// Compute initial disk
		for (int k=0; k<resolutionRadiale_; k++)
		{
			angle = 2*M_PI*k/(double)resolutionRadiale_;
			trZ[0] = cos(angle), trZ[1] = sin(angle), trZ[3] = -sin(angle), trZ[4] = cos(angle);
			point = transformationFromOrigin*(trZ*CVector3(rayon_,0.0,0.0));
            CVector3 vecPoint = initialPoint_ - point;
            point[0] += stretchingFactorWorld[0]*vecPoint[0];
            point[1] += stretchingFactorWorld[1]*vecPoint[1];
            point[2] += stretchingFactorWorld[2]*vecPoint[2];
			initialTube1->addPoint(new Vertex(point,(point-initialPoint_).Normalize()));
		}
		CVector3 nextPoint = initialPoint_ + deplacementAxial_*directionInitiale;
		computeNewBand(initialTube1,initialPoint_,nextPoint,resolutionAxiale_+2);
        

		initialTube1->computeConnectivity();
		initialTube1->computeTrianglesBarycentre();

		initialTube2 = new SpinalCord;
		directionInitiale = initialNormal2_;
		if (directionInitiale[2] == 0.0) directionInitialePerpendiculaire = CVector3(0.0,0.0,1.0);
		else directionInitialePerpendiculaire = CVector3(1.0,2.0,-(directionInitiale[0]+2*directionInitiale[1])/directionInitiale[2]).Normalize();
		refInitial = Referential(directionInitiale^directionInitialePerpendiculaire, directionInitialePerpendiculaire, directionInitiale, initialPoint_);
		transformationFromOrigin = refInitial.getTransformationInverse();
		// Compute initial disk
		for (int k=0; k<resolutionRadiale_; k++)
		{
			angle = 2*M_PI*k/(double)resolutionRadiale_;
			trZ[0] = cos(angle), trZ[1] = sin(angle), trZ[3] = -sin(angle), trZ[4] = cos(angle);
			point = transformationFromOrigin*(trZ*CVector3(rayon_,0.0,0.0));
            CVector3 vecPoint = initialPoint_ - point;
            point[0] += stretchingFactorWorld[0]*vecPoint[0];
            point[1] += stretchingFactorWorld[1]*vecPoint[1];
            point[2] += stretchingFactorWorld[2]*vecPoint[2];
			initialTube2->addPoint(new Vertex(point,(point-initialPoint_).Normalize()));
		}
		nextPoint = initialPoint_ + deplacementAxial_*directionInitiale;
		computeNewBand(initialTube2,initialPoint_,nextPoint,resolutionAxiale_+2);

		initialTube2->computeConnectivity();
		initialTube2->computeTrianglesBarycentre();

		isMeshInitialized = true;

		meanContrast = computeContrast(refInitial);
	}
	else {
		initialTube1 = new SpinalCord;
		CVector3 directionInitiale = (centerline[1]-centerline[0]).Normalize(), directionInitialePerpendiculaire;
		if (directionInitiale[2] == 0.0) directionInitialePerpendiculaire = CVector3(0.0,0.0,1.0);
		else directionInitialePerpendiculaire = CVector3(1.0,2.0,-(directionInitiale[0]+2*directionInitiale[1])/directionInitiale[2]).Normalize();
		Referential refInitial = Referential(directionInitiale^directionInitialePerpendiculaire, directionInitialePerpendiculaire, directionInitiale, centerline[0]);
		CMatrix4x4 transformationFromOrigin = refInitial.getTransformationInverse();
		double angle;
		CMatrix3x3 trZ;
		CVector3 point, normale;
		// Compute initial disk
		for (int k=0; k<resolutionRadiale_; k++)
		{
			angle = 2*M_PI*k/(double)resolutionRadiale_;
			trZ[0] = cos(angle), trZ[1] = sin(angle), trZ[3] = -sin(angle), trZ[4] = cos(angle);
			point = transformationFromOrigin*(trZ*CVector3(rayon_,0.0,0.0));
			initialTube1->addPoint(new Vertex(point,(point-centerline[0]).Normalize()));
		}
		for (unsigned int i=1; i<centerline.size(); i++)
		{
			computeNewBand(initialTube1,centerline[i-1],centerline[i],resolutionAxiale_);
		}

		initialTube1->computeConnectivity();
		initialTube1->computeTrianglesBarycentre();

		isMeshInitialized = true;
	}
}