← Back to team overview

yade-dev team mailing list archive

[svn] r1615 - trunk/extra/SpherePadder

 

Author: richefeu
Date: 2009-01-08 12:26:43 +0100 (Thu, 08 Jan 2009)
New Revision: 1615

Modified:
   trunk/extra/SpherePadder/SpherePadder.cpp
   trunk/extra/SpherePadder/SpherePadder.hpp
   trunk/extra/SpherePadder/main.cpp
Log:
SpherePadder devel...



Modified: trunk/extra/SpherePadder/SpherePadder.cpp
===================================================================
--- trunk/extra/SpherePadder/SpherePadder.cpp	2009-01-07 15:56:28 UTC (rev 1614)
+++ trunk/extra/SpherePadder/SpherePadder.cpp	2009-01-08 11:26:43 UTC (rev 1615)
@@ -10,11 +10,6 @@
 
 #include "SpherePadder.hpp"
 
-double rand01()
-{
-  return (double)rand()/(double)RAND_MAX;
-}
-
 int compare_neighbor_with_distance (const void * a, const void * b)
 {
   double d1 = (*(neighbor_with_distance *)a).distance;
@@ -37,46 +32,41 @@
                 lst.push_back(j);
                 lst.push_back(k);
                 lst.push_back(l);
-                //cout << i << ' ' << j << ' ' << k << ' ' << l << endl;
                 combination.push_back(lst);
         }
         
    max_overlap_rate = 1e-2;   
    n1 = n2 = n3 = n4 = n_densify = 0;  
-}
+   
+   //FIXME : 
+   // si on utilise la class dans un autre programme c'est peu etre pas util
+                
+   //rmin = 1e-2;
+   //rmax = 2e-2;
+   //rmoy = 0.5 * (rmin + rmax);
+   ratio = 3.0; rmoy = 0.0;
 
-
-void SpherePadder::read_data (const char* filename)
-{
-	//FIXME : 
-	// si on utilise la class dans un autre programme c'est peu etre pas util
-		
-	//rmin = 1e-2;
-	//rmax = 2e-2;
-	//rmoy = 0.5 * (rmin + rmax);
-	ratio = 3.0; rmoy = 0.0;
-
 /* FIXME
-pour le moment, l'utilisateur ne peut entre qu'un ratio.
-Le rayon des sphere depend du maillage (des longueurs des segments)
+   pour le moment, l'utilisateur ne peut entre qu'un ratio.
+   Le rayon des sphere depend du maillage (des longueurs des segments)
 */
 
-
 }
 	
 void SpherePadder::plugTetraMesh (TetraMesh * pluggedMesh)
 {
 	mesh = pluggedMesh;
 	
+        // TODO mettre ce qui suite dans une fonction 'init()'
 	// Si l'utilisateur n'a choisi qu'une valeur de ratio, 
     // on cree les valeur de rmin et rmax:
 	if (rmoy == 0 && ratio > 0)
 	{
 		rmoy = 0.125 * mesh->mean_segment_length; // 1/8
 	    rmin = (2.0 * rmoy) / (ratio + 1.0);
-	    //rmax = (2.0 * ratio * rmoy) / (ratio + 1.0); // FIXME ask jf
+	    //rmax = (2.0 * ratio * rmoy) / (ratio + 1.0); 
 		rmax = 2.0 * rmoy - rmin;
-	    //dr	 = 0.5 * (rmax - rmin); // FIXME : voir avec jf ce qu'il fait vraiment (sa version (rmax+rmin)/2)
+	    //dr	 = 0.5 * (rmax - rmin); 
 		dr = rmax - rmoy;
 	}
 }
@@ -86,15 +76,17 @@
 	// TODO check if all si ok (mesh exist...)
 	place_at_nodes();
 	place_at_segment_middle();
+        cancel_overlap();
 	place_at_barycentre_3();
-	cancel_overlap();
-        
+
         cerr << "nb spheres = " << sphere.size() << endl;
 	
 }
 
 void SpherePadder::save_mgpost (const char* name)
 {
+  cerr << "save mgp... ";
+  
   ofstream fmgpost(name);
 
   fmgpost << "<?xml version=\"1.0\"?>" << endl
@@ -102,42 +94,56 @@
 	      << "  <newcolor name=\"at nodes\"/>" << endl
 	      << "  <newcolor name=\"at segments\"/>" << endl	
 	      << "  <newcolor name=\"at faces\"/>" << endl
-              << "  <newcolor name=\"debug\"/>" << endl
+              << "  <newcolor name=\"at tetraCenters\"/>" << endl
 	      << "  <state id=\"" << 1 
 	      << "\" time=\"" << 0.0 << "\">" << endl;
 
   for (unsigned int i = 0 ; i < sphere.size() ; ++i)
-    {
-    fmgpost << "   <body>" << endl;
-    fmgpost << "    <SPHER id=\"" << i+1 << "\" col=\"" << sphere[i].type << "\" r=\"" << sphere[i].R << "\">" << endl
-		    << "     <position x=\"" << sphere[i].x << "\" y=\"" << sphere[i].y << "\" z=\"" << sphere[i].z << "\"/>" << endl	 
-            << "    </SPHER>" << endl << flush;
+  {
+        fmgpost << "   <body>" << endl;
+        fmgpost << "    <SPHER id=\"" << i+1 << "\" col=\"" << sphere[i].type << "\" r=\"" << sphere[i].R << "\">" << endl
+		        << "     <position x=\"" << sphere[i].x << "\" y=\"" << sphere[i].y << "\" z=\"" << sphere[i].z << "\"/>" << endl	 
+                << "    </SPHER>" << endl << flush;
 
-  // tmp (bricolage)
-  if (i < mesh->node.size())
-		for (unsigned int s = 0 ; s < mesh->segment.size() ; ++s)
-		{
-			if (mesh->segment[s].nodeId[0] == i)
-			{
-				fmgpost << "    <SPSPx antac=\"" << mesh->segment[s].nodeId[1] + 1 << "\"/>" << endl;
-			}
-		}
+        // tmp (bricolage)
+        if (i < mesh->node.size())
+	        for (unsigned int s = 0 ; s < mesh->segment.size() ; ++s)
+	        {
+		        if (mesh->segment[s].nodeId[0] == i)
+		        {
+			        fmgpost << "    <SPSPx antac=\"" << mesh->segment[s].nodeId[1] + 1 << "\"/>" << endl;
+		        }
+	        }
 
 
-    fmgpost << "   </body>" << endl;
-    }
+        fmgpost << "   </body>" << endl;
+   }
 
   fmgpost << "  </state>" << endl
-	      << " </mgpost>" << endl;
+	  << " </mgpost>" << endl;
 		
+  cerr << "Done" << endl;
 }
 	
+void SpherePadder::save_Rxyz (const char* name)
+{
+  cerr << "save Rxyz... ";
+  
+  ofstream file(name);
+
+  for (unsigned int i = 0 ; i < sphere.size() ; ++i)
+  {
+    file << sphere[i].R << " " << sphere[i].x << " " << sphere[i].y << " " << sphere[i].z << endl;
+  }
+                
+  cerr << "Done" << endl;
+}
+        
 void SpherePadder::place_at_nodes ()
 {
 	unsigned int segId;
 	Sphere S;
-	S.type = 0;
-	S.owner.push_back(0);
+	S.type = AT_NODE;
 	
 	cerr << "place at nodes... ";
 	
@@ -153,22 +159,20 @@
 			S.R = (S.R < mesh->segment[segId].length) ? S.R : mesh->segment[segId].length;
 		}	
 		S.R /= 4.0;
-		S.owner[0] = n;// FIXME utile ???
 		
 		S.tetraOwner = mesh->node[n].tetraOwner[0];
 		mesh->tetraedre[S.tetraOwner].sphereId.push_back(n);
 		
 		sphere.push_back(S); ++(n1);	
 	}
-	cerr << "done" << endl;
+	cerr << "Done" << endl;
 }
 
-void SpherePadder::place_at_segment_middle () // barycentre_2 ??
+void SpherePadder::place_at_segment_middle ()
 {	
 	cerr << "place at segment middle... ";
 	Sphere S;
-	S.type = 1;
-	S.owner.push_back(0);
+	S.type = AT_SEGMENT;
 	double x1,y1,z1;
 	double x2,y2,z2;
 	unsigned int id1,id2;
@@ -192,17 +196,16 @@
 		S.z = 0.5 * (z1 + z2);
 		S.R = 0.125 * mesh->segment[s].length;
 		if (S.R < rmin) S.R = rmin;
-		else if (S.R > rmax) S.R = rmoy + dr * rand01();
-		S.owner[0] = s;
-		sphere.push_back(S); ++(n2);	
+                else if (S.R > rmax) S.R = rmoy + dr * (double)rand()/(double)RAND_MAX;
 		
 		S.tetraOwner = mesh->node[id1].tetraOwner[0];
 		mesh->tetraedre[S.tetraOwner].sphereId.push_back(n0 + s);
-		
+                sphere.push_back(S); ++(n2);
+                
 		mesh->segment[s].sphereId = n0 + s;
 		
 	}	
-	cerr << "done" << endl;	
+	cerr << "Done" << endl;	
 }
 
 
@@ -210,7 +213,7 @@
 {
 	cerr << "place at barycentre 3... ";
  
-        // FIXME move the following loops in TetraMesh
+        // FIXME move the following loops in TetraMesh or at the end of place_at_segment_middle ??
 	unsigned int sphereId,faceId;
 	for (unsigned int s = 0 ; s < mesh->segment.size() ; ++s)
 	{
@@ -223,8 +226,8 @@
 	}
 	
 	Sphere S;
-	S.type = 2;
-	// todo S.owner ...
+	S.type = AT_FACE;
+
 	unsigned int ns0 = sphere.size();
         unsigned int ns = ns0; 
         const double div3 = 0.3333333333333;
@@ -248,23 +251,20 @@
 		S.R = (R1  > R2) ? R2 : R1;
 		S.R = (S.R > R3) ? R3 : S.R;
 		
-		sphere.push_back(S); ++(n3);
-
 		S.tetraOwner = mesh->node[ mesh->face[f].nodeId[0] ].tetraOwner[0];
 		mesh->tetraedre[S.tetraOwner].sphereId.push_back(ns++);
+                sphere.push_back(S); ++(n3);
 	}
-	cerr << "done" << endl;
-        
-
+	
         for (unsigned int n = (n1+n2) ; n < sphere.size() ; ++n)
         {
-          place_sphere_4contacts(n,20);
+          place_sphere_4contacts(n);
         }
-            
+        
+        cerr << "Done" << endl;  
 }
 
 
-
 double SpherePadder::distance_spheres(unsigned int i, unsigned int j)
 {
 	double lx,ly,lz;
@@ -287,7 +287,7 @@
 void SpherePadder::cancel_overlap() // FIXME rename cancel_overlaps
 {
 	
-	cerr << "cancel_overlap... ";
+	cerr << "cancel_overlaps... ";
 	unsigned int current_tetra_id,tetra_neighbor_id,j;
 	Tetraedre current_tetra, tetra_neighbor;
 	double distance,k;
@@ -310,9 +310,9 @@
 				if (sphere[j].R < 0.0) continue;
 				if (i < j)
 				{
-					while ( (distance = distance_spheres(i,j)) < distance_max)
+					while ( (distance = distance_spheres(i,j)) < distance_max )
 					{						
-						k = 1.0 + distance / (sphere[i].R+ sphere[j].R);
+						k = 1.0 + distance / (sphere[i].R + sphere[j].R);
 						sphere[i].R *= k;
 						sphere[j].R *= k;
 					}
@@ -320,13 +320,11 @@
 			}
 		}
 	}
-	cerr << "done" << endl;
+	cerr << "Done" << endl;
 }
 
 
-
-// FIXME nom : find_4contacts(...)
-unsigned int SpherePadder::place_sphere_4contacts (unsigned int sphereId, unsigned int nb_iter_max) // FIXME nb_iter_max utile ???
+unsigned int SpherePadder::place_sphere_4contacts (unsigned int sphereId)
 { 
   Sphere S = sphere[sphereId];
   unsigned int current_tetra_id = S.tetraOwner;
@@ -371,6 +369,8 @@
   }
   
   qsort(&(neighbor[0]),neighbor.size(),sizeof(neighbor_with_distance),compare_neighbor_with_distance); 
+  S.R += neighbor[0].distance;
+  sphere[sphereId].R = S.R;
   
   vector<vector<unsigned int> > possible_combination;
   for (unsigned int c = 0 ; c < combination.size() ; ++c)
@@ -385,78 +385,38 @@
   unsigned int s1,s2,s3,s4;
   unsigned int failure;
   for (unsigned int c = 0 ; c < possible_combination.size() ; ++c)
-  {
-    // check if combination is possible
-    
+  {    
     s1 = neighbor[ possible_combination[c][0] ].sphereId;
     s2 = neighbor[ possible_combination[c][1] ].sphereId;
     s3 = neighbor[ possible_combination[c][2] ].sphereId;
     s4 = neighbor[ possible_combination[c][3] ].sphereId;
-    //cout << sphereId << ' ' << s1 << ' ' << s2 << ' ' << s3 << ' ' << s4 << endl;
 
     failure = place_fifth_sphere(s1,s2,s3,s4,S);
-    //cerr << success << " " ;
+
     if (!failure)
     {
       sphere[sphereId].x = S.x;
       sphere[sphereId].y = S.y;
       sphere[sphereId].z = S.z;
       sphere[sphereId].R = S.R;
-      sphere[sphereId].type = 3; // debug
-      //cerr << "placed\n" << "R = " << S.R << endl;
       return 1;
     }
 
   }
-  //cerr << "fail\n";
+
   return 0;
 }
 
-double norm(double U[])
+double SpherePadder::distance_vector3 (double V1[],double V2[])
 {
-  int i;
-  double n = 0;
-  for (i = 0 ; i <= 2 ; i++)
-  {
-    n += U[i] * U[i];
-  }
-  return sqrt(n);
+  double D[3];
+  D[0] = V2[0] - V1[0];
+  D[1] = V2[1] - V1[1];
+  D[2] = V2[2] - V1[2];
+  return ( sqrt(D[0]*D[0] + D[1]*D[1] + D[2]*D[2]) );
 }
 
-void moins(double U[],double V[],double Rep[])
-{
-  int i;
-  for (i=0;i<=2;i++)
-  {
-    Rep[i] = V[i] - U[i]; 
-  }
-}
-
-void product(double P[3][3],double U[],double Rep[])
-{
-  double var = 0;
-  double C[3];
-  int i,j;
-  for (j = 0 ; j <= 2 ; j++)
-  {
-    for(i = 0 ; i <= 2 ; i++)
-    {
-      var = var + P[j][i]*U[i];                          
-    }                      
-    C[j] = var;
-    var = 0;
-  }
-     
-  for (i = 0 ; i <= 2 ; i++)
-  {
-    Rep[i] = C[i];
-  }
-}
-
-
-
 unsigned int SpherePadder::place_fifth_sphere(unsigned int s1, unsigned int s2, unsigned int s3, unsigned int s4, Sphere& S)
-//void calcul_5(double C1[],double R1,double C2[],double R2,double C3[],double R3,double C4[],double R4,double Rep[],double valeur_interpenetration)
 {
   double C1[3],C2[3],C3[3],C4[3];
   double R1,R2,R3,R4;
@@ -465,106 +425,95 @@
   C3[0] = sphere[s3].x ; C3[1] = sphere[s3].y ; C3[2] = sphere[s3].z ; R3 = sphere[s3].R; 
   C4[0] = sphere[s4].x ; C4[1] = sphere[s4].y ; C4[2] = sphere[s4].z ; R4 = sphere[s4].R; 
   
-  // bool -> 1 2 4 8 et return (b1 | b2 | bool2 | bool4)
-  unsigned int fail_det          = 1;
-  unsigned int fail_delta        = 2;
-  unsigned int fail_radius       = 4;
-  unsigned int fail_overlap      = 8;
-  unsigned int fail_radius_range = 16;
-  
- /*
-  *(x-x1)^2 + (y-y1)^2 + (z-z1)^2 = (r+r1)^2   (1)
-  *(x-x2)^2 + (y-y2)^2 + (z-z2)^2 = (r+r2)^2   (2)
-  *(x-x3)^2 + (y-y3)^2 + (z-z3)^2 = (r+r3)^2   (3)
-  *(x-x4)^2 + (y-y4)^2 + (z-z4)^2 = (r+r4)^2   (4)
-  */
+  unsigned int fail_det          =  1;
+  unsigned int fail_delta        =  2;
+  unsigned int fail_radius       =  4;
+  unsigned int fail_overlap      =  8;
+  unsigned int fail_gap          = 16;
+  unsigned int fail_radius_range = 32;
 
-  /* (2)-(1) */
+  // (x-x1)^2 + (y-y1)^2 + (z-z1)^2 = (r+r1)^2   (1)
+  // (x-x2)^2 + (y-y2)^2 + (z-z2)^2 = (r+r2)^2   (2)
+  // (x-x3)^2 + (y-y3)^2 + (z-z3)^2 = (r+r3)^2   (3)
+  // (x-x4)^2 + (y-y4)^2 + (z-z4)^2 = (r+r4)^2   (4)
 
-  double a = 2.0 * (C1[0]-C2[0]);
-  double b = 2.0 * (C1[1]-C2[1]);
-  double c = 2.0 * (C1[2]-C2[2]);
-  double d = 2.0 * (R1-R2);
-  double e = (C1[0]*C1[0]+C1[1]*C1[1]+C1[2]*C1[2]-R1*R1)-(C2[0]*C2[0]+C2[1]*C2[1]+C2[2]*C2[2]-R2*R2);
+  // (2)-(1)
+  double a = 2.0 * (C1[0] - C2[0]);
+  double b = 2.0 * (C1[1] - C2[1]);
+  double c = 2.0 * (C1[2] - C2[2]);
+  double d = 2.0 * (R1 - R2);
+  double e = (C1[0]*C1[0] + C1[1]*C1[1] + C1[2]*C1[2] - R1*R1) - (C2[0]*C2[0] + C2[1]*C2[1] + C2[2]*C2[2] - R2*R2);
 
-  /* (3)-(1) */
+  // (3)-(1)
+  double aa = 2.0 * (C1[0] - C3[0]);
+  double bb = 2.0 * (C1[1] - C3[1]);
+  double cc = 2.0 * (C1[2] - C3[2]);
+  double dd = 2.0 * (R1 - R3);
+  double ee = (C1[0]*C1[0] + C1[1]*C1[1] + C1[2]*C1[2] - R1*R1) - (C3[0]*C3[0] + C3[1]*C3[1] + C3[2]*C3[2] - R3*R3);
 
-  double aa = 2.0 * (C1[0]-C3[0]);
-  double bb = 2.0 * (C1[1]-C3[1]);
-  double cc = 2.0 * (C1[2]-C3[2]);
-  double dd = 2.0 * (R1-R3);
-  double ee = (C1[0]*C1[0]+C1[1]*C1[1]+C1[2]*C1[2]-R1*R1)-(C3[0]*C3[0]+C3[1]*C3[1]+C3[2]*C3[2]-R3*R3);
+  // (4)-(1)
+  double aaa = 2.0 *(C1[0] - C4[0]);
+  double bbb = 2.0 *(C1[1] - C4[1]);
+  double ccc = 2.0 *(C1[2] - C4[2]);
+  double ddd = 2.0 *(R1 - R4);
+  double eee = (C1[0]*C1[0] + C1[1]*C1[1] + C1[2]*C1[2] - R1*R1) - (C4[0]*C4[0] + C4[1]*C4[1] + C4[2]*C4[2] - R4*R4);
 
-  /* (4)-(1) */
+  // compute the determinant of matrix A (system AX = B)
+  //      [a  ,  b,  c];    [x]     [e  -  d*r]
+  //  A = [aa ,bb ,cc ];  X=[y]   B=[ee - dd*r]
+  //      [aaa,bbb,ccc];    [z]     [eee-ddd*r]
 
-  double aaa = 2.0 *(C1[0]-C4[0]);
-  double bbb = 2.0 *(C1[1]-C4[1]);
-  double ccc = 2.0 *(C1[2]-C4[2]);
-  double ddd = 2.0 *(R1-R4);
-  double eee = (C1[0]*C1[0]+C1[1]*C1[1]+C1[2]*C1[2]-R1*R1)-(C4[0]*C4[0]+C4[1]*C4[1]+C4[2]*C4[2]-R4*R4);
-
- /*
-  *calcul du determinant de la matrice A du systeme AX=B
-  *   [a  ,  b,  c];    [x]     [e  -  d*r]
-  * A=[aa ,bb ,cc ];  X=[y]   B=[ee - dd*r]
-  *   [aaa,bbb,ccc];    [z]     [eee-ddd*r]
-  * developpement par rapport � la 1�re colonne     
-  */
-
-  double DET=a*(bb*ccc-bbb*cc)-aa*(b*ccc-bbb*c)+aaa*(b*cc-bb*c);
+  double DET = a*(bb*ccc-bbb*cc) - aa*(b*ccc-bbb*c) + aaa*(b*cc-bb*c);
   double R = 0.0;
   double centre[3];
   if (DET != 0.0)
   {
+    //        [a11,a12,a13]
+    // A^-1 = [a21,a22,a23] * 1/det(A)
+    //        [a31,a32,a33]  
+          
+    double inv_DET = 1.0/DET;
+    double a11 =  (bb*ccc-bbb*cc) * inv_DET;
+    double a12 = -(b*ccc-bbb*c)   * inv_DET;
+    double a13 =  (b*cc-bb*c)     * inv_DET;
+    double a21 = -(aa*ccc-aaa*cc) * inv_DET;
+    double a22 =  (a*ccc-aaa*c)   * inv_DET;
+    double a23 = -(a*cc-aa*c)     * inv_DET;
+    double a31 =  (aa*bbb-aaa*bb) * inv_DET;
+    double a32 = -(a*bbb-aaa*b)   * inv_DET;
+    double a33 =  (a*bb-aa*b)     * inv_DET;
 
-   /*
-    * Calcul de A^-1
-    *       [a11,a12,a13]
-    * A^-1= [a21,a22,a23] *1/det(A)
-    *       [a31,a32,a33]        
-    */
+    double xa = -(a11*d + a12*dd + a13*ddd);
+    double xb =  (a11*e + a12*ee + a13*eee);
 
-    double a11=(bb*ccc-bbb*cc)/DET;
-    double a12=-(b*ccc-bbb*c)/DET;
-    double a13=(b*cc-bb*c)/DET;
-    double a21=-(aa*ccc-aaa*cc)/DET;
-    double a22=(a*ccc-aaa*c)/DET;
-    double a23=-(a*cc-aa*c)/DET;
-    double a31=(aa*bbb-aaa*bb)/DET;
-    double a32=-(a*bbb-aaa*b)/DET;
-    double a33=(a*bb-aa*b)/DET;
+    double ya = -(a21*d + a22*dd + a23*ddd);
+    double yb =  (a21*e + a22*ee + a23*eee);
 
-    double xa=-(a11*d+a12*dd+a13*ddd);
-    double xb=(a11*e+a12*ee+a13*eee);
+    double za = -(a31*d + a32*dd + a33*ddd);
+    double zb =  (a31*e + a32*ee + a33*eee);
 
-    double ya=-(a21*d+a22*dd+a23*ddd);
-    double yb=(a21*e+a22*ee+a23*eee);
+    // Replace x,y and z in Equation (1) and solve the second order equation A*r^2 + B*r + C = 0
 
-    double za=-(a31*d+a32*dd+a33*ddd);
-    double zb=(a31*e+a32*ee+a33*eee);
+    double A = xa*xa + ya*ya + za*za - 1.0;
+    double B = 2.0*(xa*(xb-C1[0])) + 2.0*(ya*(yb-C1[1])) + 2.0*(za*(zb-C1[2])) - 2.0*R1;
+    double C = (xb-C1[0])*(xb-C1[0]) + (yb-C1[1])*(yb-C1[1]) + (zb-C1[2])*(zb-C1[2]) - R1*R1;
 
-/*On remplace x,y et z dans l'�quation (1) et on r�soud l'�quation
- * du second degr� en r A*r^2+B*r+C=0 */
-
-    double A=xa*xa+ya*ya+za*za-1;
-    double B=2*(xa*(xb-C1[0]))+2*(ya*(yb-C1[1]))+2*(za*(zb-C1[2]))-2*R1;
-    double C=(xb-C1[0])*(xb-C1[0])+(yb-C1[1])*(yb-C1[1])+(zb-C1[2])*(zb-C1[2])-R1*R1;
-
     double DELTA = B*B - 4.0*A*C;
     double RR1,RR2;
 
     if (DELTA >= 0.0)
     {
-      RR1 = (-B + sqrt(DELTA)) / (2.0*A);
-      RR2 = (-B - sqrt(DELTA)) / (2.0*A);
+      double inv_2A = 1.0 / (2.0*A);
+      double sqrt_DELTA = sqrt(DELTA);
+      RR1 = (-B + sqrt_DELTA) * inv_2A;
+      RR2 = (-B - sqrt_DELTA) * inv_2A;
     }
     else return fail_delta;
-    
 
     if      (RR1 > 0.0) R = RR1;
     else if (RR2 > 0.0) R = RR2;
-
-    if (RR1 <= 0.0 && RR2 <= 0.0) return fail_radius;
+    else if (RR1 <= 0.0 && RR2 <= 0.0) return fail_radius;
+    
     if (R < rmin || R > rmax) return fail_radius_range;
 
     centre[0] = xa * R + xb;
@@ -572,34 +521,32 @@
     centre[2] = za * R + zb;
   }
   else return fail_det;
+  
+  // Check interpenetration between spheres
 
-/*------------------------------------------------------------------------------
- * Interpenetration entre spheres (x, y , z, rayon )
- *----------------------------------------------------------------------------*/
-  // U4 = C4 - centre
-  double U4[3];
-  moins(centre,C4,U4);
-
-  // U3 = C3 - centre
-  double U3[3];
-  moins(centre,C3,U3);
-
-  // U2 = C2 - centre
-  double U2[3];
-  moins(centre,C2,U2);
-
-  // U1 = C1 - centre
-  double U1[3];
-  moins(centre,C1,U1);
-
-  if (    (norm(U4)-(R4+R) < -0.5 * max_overlap_rate*(R+R4)) 
-       || (norm(U3)-(R3+R) < -0.5 * max_overlap_rate*(R+R3))
-       || (norm(U2)-(R2+R) < -0.5 * max_overlap_rate*(R+R2)) 
-       || (norm(U1)-(R1+R) < -0.5 * max_overlap_rate*(R+R1)) )
-  {
-    return fail_overlap;
-  }
-
+  double distance1 = distance_vector3 (centre,C1) - (R + R1);
+  double distance2 = distance_vector3 (centre,C2) - (R + R2);
+  double distance3 = distance_vector3 (centre,C3) - (R + R3);
+  double distance4 = distance_vector3 (centre,C4) - (R + R4);
+  
+  double half_distance_rate = -0.5 * max_overlap_rate;
+  if (     ( distance1 < half_distance_rate * (R + R1)) 
+        || ( distance2 < half_distance_rate * (R + R2))
+        || ( distance3 < half_distance_rate * (R + R3)) 
+        || ( distance4 < half_distance_rate * (R + R4)) )
+  { return fail_overlap; }
+  
+  // The distance must not be too large
+  double distance_max = max_overlap_rate * rmin; // FIXME should be imposed by the user? for example max_gap_rate
+  if (     ( distance1 > distance_max) 
+        || ( distance2 > distance_max)
+        || ( distance3 > distance_max) 
+        || ( distance4 > distance_max) )
+  { return fail_gap; } 
+  
+  
+  // TODO check if there is no NaN or Inf
+  
   S.x = centre[0];
   S.y = centre[1];
   S.z = centre[2];
@@ -608,14 +555,16 @@
   return 0;
 }
 
-
-
-
-
-
-
-
+void SpherePadder::place_at_barycentre_4 ()
+{
+// TODO 
+}
+    
 void SpherePadder::place_at_tetra_centers ()
 {
 // TODO	
 }
+
+
+
+

Modified: trunk/extra/SpherePadder/SpherePadder.hpp
===================================================================
--- trunk/extra/SpherePadder/SpherePadder.hpp	2009-01-07 15:56:28 UTC (rev 1614)
+++ trunk/extra/SpherePadder/SpherePadder.hpp	2009-01-08 11:26:43 UTC (rev 1615)
@@ -13,14 +13,15 @@
 
 #include "TetraMesh.hpp"
 
+enum SphereType {AT_NODE,AT_SEGMENT,AT_FACE,AT_TETRA_CENTER};
+
+
 struct Sphere
 {
 	double                x,y,z,R;
-	unsigned int          type; // FIXME utiliser un enum ??
+	//unsigned int          type; // FIXME utiliser un enum ??
+        SphereType     type; 
 	unsigned int          tetraOwner;
-	vector<unsigned int>  owner; // a documenter... FIXME necessaire ?
-	// type = 0 => owner = nodeId
-	// type = 1 => owner = segId
 };
 
 struct Neighbor
@@ -40,20 +41,18 @@
         	
         vector<vector<unsigned int> > combination;
   
-	double distance_spheres (unsigned int i, unsigned int j);
-        double distance_centre_spheres(Sphere& S1, Sphere& S2);
-	void place_at_nodes ();
-        void place_at_segment_middle (); // place_at_barycentre_2 ??
-	void place_at_barycentre_3 ();
-        // void place_at_barycentre_4 ();
-	void cancel_overlap ();
+	double       distance_spheres (unsigned int i, unsigned int j);
+        double       distance_centre_spheres(Sphere& S1, Sphere& S2);
+        double       distance_vector3 (double V1[],double V2[]);
+	void         place_at_nodes ();
+        void         place_at_segment_middle ();
+	void         place_at_barycentre_3 ();
+        void         place_at_tetra_centers ();
+        void         place_at_barycentre_4 ();
+	void         cancel_overlap ();
         unsigned int place_fifth_sphere(unsigned int s1, unsigned int s2, unsigned int s3, unsigned int s4, Sphere& S);
-        unsigned int place_sphere_4contacts (unsigned int sphereId, unsigned int nb_iter_max);
-        
-	void place_at_tetra_centers ();
-	
-public:
-  
+        unsigned int place_sphere_4contacts (unsigned int sphereId);
+        	 
 	double rmin,rmax,rmoy,dr;
 	double ratio;
 	double max_overlap_rate;
@@ -62,14 +61,17 @@
 	TetraMesh * mesh;
 	vector<Sphere> sphere;
 
-	void read_data (const char* filename);
+ public:
+   
 	void plugTetraMesh (TetraMesh * mesh);
 	void save_mgpost (const char* name);
-	//void save_Rxyz (const char* name);
+	void save_Rxyz   (const char* name);
 	
         SpherePadder();
+        // TODO destructor that clean TetraMesh*
         
-	void pad_5 ();		
+	void pad_5 ();
+        // void densify ();	
 };
 
 

Modified: trunk/extra/SpherePadder/main.cpp
===================================================================
--- trunk/extra/SpherePadder/main.cpp	2009-01-07 15:56:28 UTC (rev 1614)
+++ trunk/extra/SpherePadder/main.cpp	2009-01-08 11:26:43 UTC (rev 1615)
@@ -12,16 +12,17 @@
 
 int main()
 {
-	SpherePadder * padder = new SpherePadder();
-	padder->read_data("padding.dat");
 	TetraMesh * mesh = new TetraMesh();
-	mesh->read_data("small.msh");
+	mesh->read_data("test.msh");
+        
+        SpherePadder * padder = new SpherePadder();
 	padder->plugTetraMesh(mesh);
 	
 	padder->pad_5();
 	
 	padder->save_mgpost("mgp.out.001");
-	
+        padder->save_Rxyz("out.Rxyz");
+        
 	return 0;
 }