From 68f652a38ccbe2ff70813f90af88ae9eb5d73db7 Mon Sep 17 00:00:00 2001
From: Jean-Francois Remacle <jean-francois.remacle@uclouvain.be>
Date: Sat, 25 Oct 2014 20:27:16 +0000
Subject: [PATCH] fixed several bugs

---
 Common/OpenFile.cpp                           |   3 +-
 Common/StringUtils.cpp                        |   2 +-
 Geo/GModel.cpp                                |   3 +
 Geo/GModelIO_INP.cpp                          |   5 +-
 Geo/Geo.cpp                                   |  28 +-
 Geo/GeoInterpolation.cpp                      |  49 +-
 Mesh/BackgroundMesh.cpp                       | 160 ++++-
 Mesh/BackgroundMesh.h                         |   9 +-
 Mesh/meshGFace.cpp                            |   3 +-
 Mesh/meshGFaceDelaunayInsertion.cpp           |  11 +-
 Mesh/meshGFaceOptimize.cpp                    |   5 +-
 Mesh/meshRefine.cpp                           | 581 ++++++++++++++-
 Mesh/surfaceFiller.cpp                        |   8 +-
 Numeric/CMakeLists.txt                        |   2 +
 Numeric/JacobianBasis.cpp                     |   3 +-
 Numeric/Numeric.cpp                           | 176 ++---
 Numeric/Numeric.h                             |   6 +-
 Numeric/simpleFunction.h                      |  27 +-
 Solver/helmholtzTerm.h                        |   3 +-
 benchmarks/2d/brombo2.geo                     |   3 +
 benchmarks/2d/machine2/Machine.geo            |   2 +-
 benchmarks/2d/many_holes.geo                  |   1 +
 benchmarks/3d/Cube-01.geo                     |   8 +-
 contrib/HighOrderMeshOptimizer/OptHOM.cpp     | 673 +++++++++++++++++-
 contrib/HighOrderMeshOptimizer/OptHOM.h       |  24 +-
 .../OptHomIntegralBoundaryDist.cpp            |  32 +-
 .../OptHomIntegralBoundaryDist.h              |   1 +
 contrib/HighOrderMeshOptimizer/OptHomMesh.cpp | 114 ++-
 contrib/HighOrderMeshOptimizer/OptHomMesh.h   |  17 +
 contrib/HighOrderMeshOptimizer/OptHomRun.cpp  |  30 +-
 contrib/HighOrderMeshOptimizer/OptHomRun.h    |   1 +
 31 files changed, 1748 insertions(+), 242 deletions(-)

diff --git a/Common/OpenFile.cpp b/Common/OpenFile.cpp
index b6f67986bc..728efd8be3 100644
--- a/Common/OpenFile.cpp
+++ b/Common/OpenFile.cpp
@@ -503,14 +503,13 @@ int MergeFile(const std::string &fileName, bool warnIfMissing, bool setWindowTit
   Msg::ImportPhysicalsAsOnelabRegions();
 
   if(!status) Msg::Error("Error loading '%s'", fileName.c_str());
-  Msg::StatusBar(true, "Done reading '%s'", fileName.c_str());
+  Msg::StatusBar(true, "Done reading '%s'", fileName.c_str());  
 
   CTX::instance()->fileread = true;
 
   // merge the associated option file if there is one
   if(!StatFile(fileName + ".opt"))
     MergeFile(fileName + ".opt");
-
   return status;
 }
 
diff --git a/Common/StringUtils.cpp b/Common/StringUtils.cpp
index f609237f6f..dfeb1a5ac3 100644
--- a/Common/StringUtils.cpp
+++ b/Common/StringUtils.cpp
@@ -96,7 +96,7 @@ std::vector<std::string> SplitFileName(const std::string &fileName)
   int islash = (int)fileName.find_last_of("/\\");
   if(idot == (int)std::string::npos) idot = -1;
   if(islash == (int)std::string::npos) islash = -1;
-  std::vector<std::string> s(3);
+  std::vector<std::string> s; s.resize(3); // JFR DO NOT CHANGE TO std::vector<std::string> s(3), it segfaults while destructor si called 
   if(idot > 0)
     s[2] = fileName.substr(idot);
   if(islash > 0)
diff --git a/Geo/GModel.cpp b/Geo/GModel.cpp
index 79580ee9a5..32a2909757 100644
--- a/Geo/GModel.cpp
+++ b/Geo/GModel.cpp
@@ -3608,3 +3608,6 @@ void GModel::setCompoundVisibility()
   }
 
 }
+
+
+
diff --git a/Geo/GModelIO_INP.cpp b/Geo/GModelIO_INP.cpp
index 216d70175f..e36d5641dd 100644
--- a/Geo/GModelIO_INP.cpp
+++ b/Geo/GModelIO_INP.cpp
@@ -23,7 +23,7 @@ static void writeElementsINP(FILE *fp, GEntity *ge, std::vector<T*> &elements,
     if(typ){
       const char *str = (ge->dim() == 3) ? "Volume" : (ge->dim() == 2) ?
         "Surface" : (ge->dim() == 1) ? "Line" : "Point";
-      fprintf(fp, "*Element, type=%s, ELSET=%s%d\n", typ, str, ge->tag());
+      fprintf(fp, "*ELEMENT, type=%s, ELSET=%s%d\n", typ, str, ge->tag());
       for(unsigned int i = 0; i < elements.size(); i++)
         elements[i]->writeINP(fp, elements[i]->getNum());
     }
@@ -62,11 +62,12 @@ int GModel::writeINP(const std::string &name, bool saveAll, bool saveGroupsOfNod
   fprintf(fp, "*Heading\n");
   fprintf(fp, " %s\n", name.c_str());
 
-  fprintf(fp, "*Node\n");
+  fprintf(fp, "*NODE\n");
   for(unsigned int i = 0; i < entities.size(); i++)
     for(unsigned int j = 0; j < entities[i]->mesh_vertices.size(); j++)
       entities[i]->mesh_vertices[j]->writeINP(fp, scalingFactor);
 
+  fprintf(fp, "******* E L E M E N T S *************\n");
   for(viter it = firstVertex(); it != lastVertex(); ++it){
     writeElementsINP(fp, *it, (*it)->points, saveAll);
   }
diff --git a/Geo/Geo.cpp b/Geo/Geo.cpp
index dc09d2882f..9e9552cd2b 100644
--- a/Geo/Geo.cpp
+++ b/Geo/Geo.cpp
@@ -455,14 +455,14 @@ Curve *Create_Curve(int Num, int Typ, int Order, List_T *Liste,
                          {1.0, -2.5, 2.0, -0.5},
                          {-0.5, 0.0, 0.5, 0.0},
                          {0.0, 1.0, 0.0, 0.0} };
-  double matbs[4][4] = { {-1.0, 3, -3, 1},
-                         {3, -6, 3.0, 0},
-                         {-3, 0.0, 3, 0.0},
-                         {1, 4, 1, 0.0} };
-  double matbez[4][4] = { {-1.0, 3, -3, 1},
-                          {3, -6, 3.0, 0},
-                          {-3, 3.0, 0, 0.0},
-                          {1, 0, 0, 0.0} };
+  double matbs[4][4] = { {-1, 3, -3, 1},
+                         { 3,-6,  3, 0},
+                         {-3, 0,  3, 0},
+                         { 1, 4,  1, 0} };
+  double matbez[4][4] = { {-1, 3,-3, 1},
+                          { 3,-6, 3, 0},
+                          {-3, 3, 0, 0},
+                          { 1, 0, 0, 0} };
 
   Curve *pC = new Curve;
   pC->Color.type = 0;
@@ -4251,10 +4251,12 @@ void setSurfaceEmbeddedCurves(Surface *s, List_T *curves)
 
     for(int j = 0; j < List_Nbr(s->EmbeddedCurves) + List_Nbr(s->Generatrices); j++) {
       Curve *cDejaInSurf;
+      //      Msg::Info("hopla1 %d %d %d",j,List_Nbr(s->EmbeddedCurves), List_Nbr(s->Generatrices));
       if (j < s->EmbeddedCurves->n)
         List_Read(s->EmbeddedCurves, j, &cDejaInSurf);
       else
         List_Read(s->Generatrices, j-s->EmbeddedCurves->n, &cDejaInSurf);
+      //      Msg::Info("hopla2 %d",j);
       if (cDejaInSurf->Typ != MSH_SEGM_LINE)
         // compute intersections only avalaible for straight lines
         continue;
@@ -4285,6 +4287,8 @@ void setSurfaceEmbeddedCurves(Surface *s, List_T *curves)
           List_Read(cToAddInSurf->Control_Points, l, &w1);
           List_Read(cToAddInSurf->Control_Points, l+1, &w2);
 
+	  if (w1 == v1 || w1 == v2 || w2 == v1 || w2 == v2)continue; 
+
           SPoint3 q1 = SPoint3(w1->Pos.X, w1->Pos.Y, w1->Pos.Z);
           SPoint3 q2 = SPoint3(w2->Pos.X, w2->Pos.Y, w2->Pos.Z);
 
@@ -4296,7 +4300,12 @@ void setSurfaceEmbeddedCurves(Surface *s, List_T *curves)
 
           double x[2];
           int inters = intersection_segments(p3, p4, q3, q4, x);
-
+	  /*
+	   printf("%d %d vs %d %d\n",w1->Num,w2->Num,v1->Num,v2->Num);
+	  printf("%22.15E %22.15E %22.15E -- %22.15E %22.15E %22.15E\n",p3.x(),p3.y(),p3.z(),p4.x(),p4.y(),p4.z());
+	  printf("%22.15E %22.15E %22.15E -- %22.15E %22.15E %22.15E\n",q3.x(),q3.y(),q3.z(),q4.x(),q4.y(),q4.z());
+	  printf("%22.15E %22.15E\n",x[0],x[1]);
+	  */
           if (inters && x[0] != 0. && x[1] != 0. && x[0] != 1. && x[1] != 1.){
             SPoint3 p = SPoint3( (1.-x[0])*p3.x() + x[0]*p4.x(),
                                  (1.-x[0])*p3.y() + x[0]*p4.y(),
@@ -4410,6 +4419,7 @@ void setSurfaceEmbeddedCurves(Surface *s, List_T *curves)
     }
     List_Add(s->EmbeddedCurves, &cToAddInSurf);
   }
+  //  Msg::Info("coucou2");
 }
 
 void setVolumeEmbeddedSurfaces(Volume *v, List_T *surfaces)
diff --git a/Geo/GeoInterpolation.cpp b/Geo/GeoInterpolation.cpp
index 37c5b98c78..27668190c5 100644
--- a/Geo/GeoInterpolation.cpp
+++ b/Geo/GeoInterpolation.cpp
@@ -12,6 +12,45 @@
 
 #define SQU(a)      ((a)*(a))
 
+// Cubic spline : 
+
+static void InterpolateBezier(Vertex *v[4], double t, Vertex &V)
+{
+  V.lc = (1 - t) * v[1]->lc + t * v[2]->lc;
+  V.w = (1 - t) * v[1]->w + t * v[2]->w;
+  const double tt = 1.-t;
+  const double s[4] = {tt*tt*tt, 3*t*tt*tt,3*t*t*tt,t*t*t};
+  V.Pos.X = s[0]*v[0]->Pos.X+s[1]*v[1]->Pos.X+s[2]*v[2]->Pos.X+s[3]*v[3]->Pos.X;
+  V.Pos.Y = s[0]*v[0]->Pos.Y+s[1]*v[1]->Pos.Y+s[2]*v[2]->Pos.Y+s[3]*v[3]->Pos.Y;
+  V.Pos.Z = s[0]*v[0]->Pos.Z+s[1]*v[1]->Pos.Z+s[2]*v[2]->Pos.Z+s[3]*v[3]->Pos.Z;
+}
+
+static Vertex InterpolateCubicSpline(Vertex *v[4], double t)
+{
+  Vertex V;
+  V.lc = (1 - t) * v[1]->lc + t * v[2]->lc;
+  V.w = (1 - t) * v[1]->w + t * v[2]->w;
+  const double tt = 1.-t;
+  const double  t3 = t*t*t;
+  const double s[4] = {tt*tt*tt, 3*t3-6*t*t+4,-3*t3+3*t*t+3*t+1,t3};
+  V.Pos.X = (s[0]*v[0]->Pos.X+s[1]*v[1]->Pos.X+s[2]*v[2]->Pos.X+s[3]*v[3]->Pos.X)/6.0;
+  V.Pos.Y = (s[0]*v[0]->Pos.Y+s[1]*v[1]->Pos.Y+s[2]*v[2]->Pos.Y+s[3]*v[3]->Pos.Y)/6.0;
+  V.Pos.Z = (s[0]*v[0]->Pos.Z+s[1]*v[1]->Pos.Z+s[2]*v[2]->Pos.Z+s[3]*v[3]->Pos.Z)/6.0;
+  return V;
+}
+
+static void InterpolateCatmullRom(Vertex *v[4], double t, Vertex &V)
+{
+  V.lc = (1 - t) * v[1]->lc + t * v[2]->lc;
+  V.w = (1 - t) * v[1]->w + t * v[2]->w;
+  const double t2 = t*t;
+  const double  t3 = t*t*t;
+  const double s[4] = {-.5*t3+t2-.5*t, 1.5*t3-2.5*t2+1,-1.5*t3+2*t2+.5*t,0.5*t3-0.5*t2};
+  V.Pos.X = s[0]*v[0]->Pos.X+s[1]*v[1]->Pos.X+s[2]*v[2]->Pos.X+s[3]*v[3]->Pos.X;
+  V.Pos.Y = s[0]*v[0]->Pos.Y+s[1]*v[1]->Pos.Y+s[2]*v[2]->Pos.Y+s[3]*v[3]->Pos.Y;
+  V.Pos.Z = s[0]*v[0]->Pos.Z+s[1]*v[1]->Pos.Z+s[2]*v[2]->Pos.Z+s[3]*v[3]->Pos.Z;
+}
+
 static Vertex InterpolateCubicSpline(Vertex *v[4], double t, double mat[4][4],
                                      int derivee, double t1, double t2)
 {
@@ -198,8 +237,9 @@ static Vertex InterpolateUBS(Curve *Curve, double u, int derivee)
     V.Pos.Z = pt.z();
     return V;
   }
-  else
-    return InterpolateCubicSpline(v, t, Curve->mat, derivee, t1, t2);
+  else    
+    //    return InterpolateCubicSpline(v, t, Curve->mat, derivee, t1, t2);
+    return InterpolateCubicSpline(v, t);
 }
 
 // Non Uniform BSplines
@@ -448,8 +488,9 @@ Vertex InterpolateCurve(Curve *c, double u, int derivee)
       V.Pos.Y = pt.y();
       V.Pos.Z = pt.z();
     }
-    else
-      V = InterpolateCubicSpline(v, t, c->mat, 0, t1, t2);
+    else  
+      InterpolateCatmullRom(v, t, V);
+	//      V = InterpolateCubicSpline(v, t, c->mat, 0, t1, t2);
     break;
 
   case MSH_SEGM_BND_LAYER:
diff --git a/Mesh/BackgroundMesh.cpp b/Mesh/BackgroundMesh.cpp
index 70509ca5f0..b2ac9665f6 100644
--- a/Mesh/BackgroundMesh.cpp
+++ b/Mesh/BackgroundMesh.cpp
@@ -509,7 +509,8 @@ backgroundMesh::backgroundMesh(GFace *_gf, bool cfd)
       else newv = it->second;
       news[j] = newv;
     }
-    _triangles.push_back(new MTriangle(news[0],news[1],news[2]));
+    MTriangle *T2D = new MTriangle(news[0],news[1],news[2]);
+    _triangles.push_back(T2D);
   }
 
 #if defined(HAVE_ANN)
@@ -569,8 +570,9 @@ backgroundMesh::~backgroundMesh()
 }
 
 static void propagateValuesOnFace(GFace *_gf,
-    std::map<MVertex*,double> &dirichlet,
-    bool in_parametric_plane = false)
+                                  std::map<MVertex*,double> &dirichlet,
+				  simpleFunction<double> *ONE,
+				  bool in_parametric_plane = false)
 {
 #if defined(HAVE_SOLVER)
   linearSystem<double> *_lsys = 0;
@@ -616,8 +618,7 @@ static void propagateValuesOnFace(GFace *_gf,
     myAssembler.numberVertex(*it, 0, 1);
 
   // Assemble
-  simpleFunction<double> ONE(1.0);
-  laplaceTerm l(0, 1, &ONE);
+  laplaceTerm l(0, 1, ONE);
   for (unsigned int k = 0; k < _gf->triangles.size(); k++){
     MTriangle *t = _gf->triangles[k];
     SElement se(t);
@@ -670,10 +671,11 @@ void backgroundMesh::propagate1dMesh(GFace *_gf)
           }
         }
       }
-    }
+    }  
   }
 
-  propagateValuesOnFace(_gf, sizes);
+  simpleFunction<double> ONE(1.0);
+  propagateValuesOnFace(_gf, sizes,&ONE);
 
   std::map<MVertex*,MVertex*>::iterator itv2 = _2Dto3D.begin();
   for ( ; itv2 != _2Dto3D.end(); ++itv2){
@@ -771,10 +773,93 @@ inline double myAngle (const SVector3 &a, const SVector3 &b, const SVector3 &d){
   return atan2 (sinTheta,cosTheta);
 }
 
+// smoothness = h * (|grad (cos 4 a)| + |grad (sin 4 a)|) 
+// smoothness is of order 1 if not smooth
+// smoothness is of order h/L if smooth
+// h --> mesh size
+// L --> domain size
+double backgroundMesh::getSmoothness(MElement *e) 
+{
+  MVertex *v0 = _3Dto2D[e->getVertex(0)];
+  MVertex *v1 = _3Dto2D[e->getVertex(1)];
+  MVertex *v2 = _3Dto2D[e->getVertex(2)];
+  std::map<MVertex*,double> :: const_iterator i0 = _angles.find (v0);
+  std::map<MVertex*,double> :: const_iterator i1 = _angles.find (v1);
+  std::map<MVertex*,double> :: const_iterator i2 = _angles.find (v2);
+  double a[3] = {cos(4*i0->second),cos(4*i1->second),cos(4*i2->second)}; 
+  double b[3] = {sin(4*i0->second),sin(4*i1->second),sin(4*i2->second)}; 
+  //      printf("coucou\n");
+  double f[3];
+  e->interpolateGrad(a,0,0,0,f);      
+  const double gradcos = sqrt (f[0]*f[0]+f[1]*f[1]+f[2]*f[2]);
+  e->interpolateGrad(b,0,0,0,f);      
+  const double gradsin = sqrt (f[0]*f[0]+f[1]*f[1]+f[2]*f[2]);
+  const double h = e->maxEdge();
+  return (gradcos /*+ gradsin*/) * h;
+}
+
+double backgroundMesh::getSmoothness(double u, double v, double w)
+{
+  MElement *e = _octree->find(u, v, w, 2, true);
+  if (!e) return -1.0;
+  MVertex *v0 = e->getVertex(0);
+  MVertex *v1 = e->getVertex(1);
+  MVertex *v2 = e->getVertex(2);
+  std::map<MVertex*,double> :: const_iterator i0 = _angles.find (v0);
+  std::map<MVertex*,double> :: const_iterator i1 = _angles.find (v1);
+  std::map<MVertex*,double> :: const_iterator i2 = _angles.find (v2);
+  double a[3] = {cos(4*i0->second),cos(4*i1->second),cos(4*i2->second)}; 
+  double b[3] = {sin(4*i0->second),sin(4*i1->second),sin(4*i2->second)}; 
+  //      printf("coucou\n");
+  double f[3];
+  e->interpolateGrad(a,0,0,0,f);      
+  const double gradcos = sqrt (f[0]*f[0]+f[1]*f[1]+f[2]*f[2]);
+  e->interpolateGrad(b,0,0,0,f);      
+  const double gradsin = sqrt (f[0]*f[0]+f[1]*f[1]+f[2]*f[2]);
+  const double h = e->maxEdge();
+  return (gradcos /*+ gradsin*/) * h;
+}
 
 void backgroundMesh::propagateCrossField(GFace *_gf)
 {
+  //  printf("coucou\n");
+  propagateCrossFieldHJ (_gf);
+  // solve the non liear problem
+  constantPerElement<double> C;
+  int ITER = 0;
+  //  int NSMOOTH =  _gf->triangles.size();
+  while(0){
+    //    int NSMOOTH_NOW = 0;
+    for (unsigned int i = 0; i < _gf->triangles.size(); i++){
+      double smoothness = getSmoothness (_gf->triangles[i]);
+      double val = smoothness < .5 ? 1.0 : 1.e-3 ;//exp(-absf/10);      
+      C.set(_gf->triangles[i],val);
+    }
+    //    if (NSMOOTH_NOW == NSMOOTH) break;
+    //    NSMOOTH = NSMOOTH_NOW;
+    //    break;
+    _angles.clear();
+    propagateCrossField (_gf,&C);
+    if (++ITER > 0)break;
+  }
+  //  printf("converged in %d iterations\n",ITER);
+  char name[256];
+  sprintf(name,"cross-%d-%d.pos",_gf->tag(),ITER);
+  print(name,0,1);
+  sprintf(name,"smooth-%d-%d.pos",_gf->tag(),ITER);
+  print(name,_gf,2);
+
+
+}
+
+void backgroundMesh::propagateCrossFieldHJ(GFace *_gf)
+{
+  simpleFunction<double> ONE(1.0);
+  propagateCrossField (_gf, &ONE);
 
+}
+void backgroundMesh::propagateCrossField(GFace *_gf, simpleFunction<double> *ONE)
+{
   std::map<MVertex*,double> _cosines4,_sines4;
 
   std::list<GEdge*> e;
@@ -816,8 +901,8 @@ void backgroundMesh::propagateCrossField(GFace *_gf)
     }
   }
 
-  propagateValuesOnFace(_gf,_cosines4,false);
-  propagateValuesOnFace(_gf,_sines4,false);
+  propagateValuesOnFace(_gf,_cosines4,ONE,false);
+  propagateValuesOnFace(_gf,_sines4,ONE,false);
 
   std::map<MVertex*,MVertex*>::iterator itv2 = _2Dto3D.begin();
   for ( ; itv2 != _2Dto3D.end(); ++itv2){
@@ -863,7 +948,7 @@ void backgroundMesh::updateSizes(GFace *_gf)
     }
   }
   const double _beta = 1.3;
-  for (int i=0;i<0;i++){
+  for (int i=0;i<3;i++){
     std::set<MEdge,Less_Edge>::iterator it = edges.begin();
     for ( ; it != edges.end(); ++it){
       MVertex *v0 = it->getVertex(0);
@@ -980,32 +1065,45 @@ double backgroundMesh::getAngle(double u, double v, double w) const
 }
 
 void backgroundMesh::print(const std::string &filename, GFace *gf,
-    const std::map<MVertex*,double> &_whatToPrint) const
+                           const std::map<MVertex*,double> &_whatToPrint, int smooth) 
 {
   FILE *f = Fopen (filename.c_str(),"w");
   fprintf(f,"View \"Background Mesh\"{\n");
-  for(unsigned int i=0;i<_triangles.size();i++){
-    MVertex *v1 = _triangles[i]->getVertex(0);
-    MVertex *v2 = _triangles[i]->getVertex(1);
-    MVertex *v3 = _triangles[i]->getVertex(2);
-    std::map<MVertex*,double>::const_iterator itv1 = _whatToPrint.find(v1);
-    std::map<MVertex*,double>::const_iterator itv2 = _whatToPrint.find(v2);
-    std::map<MVertex*,double>::const_iterator itv3 = _whatToPrint.find(v3);
-    if (!gf){
+  if (smooth){
+    for(unsigned int i=0;i<gf->triangles.size();i++){
+      MVertex *v1 = gf->triangles[i]->getVertex(0);
+      MVertex *v2 = gf->triangles[i]->getVertex(1);
+      MVertex *v3 = gf->triangles[i]->getVertex(2);
+      double x = getSmoothness (gf->triangles[i]);
       fprintf(f,"ST(%g,%g,%g,%g,%g,%g,%g,%g,%g) {%g,%g,%g};\n",
-          v1->x(),v1->y(),v1->z(),
-          v2->x(),v2->y(),v2->z(),
-          v3->x(),v3->y(),v3->z(),itv1->second,itv2->second,itv3->second);
+	      v1->x(),v1->y(),v1->z(),
+	      v2->x(),v2->y(),v2->z(),
+	      v3->x(),v3->y(),v3->z(),x,x,x);
     }
-    else {
-
-      GPoint p1 = gf->point(SPoint2(v1->x(),v1->y()));
-      GPoint p2 = gf->point(SPoint2(v2->x(),v2->y()));
-      GPoint p3 = gf->point(SPoint2(v3->x(),v3->y()));
-      fprintf(f,"ST(%g,%g,%g,%g,%g,%g,%g,%g,%g) {%g,%g,%g};\n",
-          p1.x(),p1.y(),p1.z(),
-          p2.x(),p2.y(),p2.z(),
-          p3.x(),p3.y(),p3.z(),itv1->second,itv2->second,itv3->second);
+  }
+  else {
+    for(unsigned int i=0;i<_triangles.size();i++){
+      MVertex *v1 = _triangles[i]->getVertex(0);
+      MVertex *v2 = _triangles[i]->getVertex(1);
+      MVertex *v3 = _triangles[i]->getVertex(2);
+      std::map<MVertex*,double>::const_iterator itv1 = _whatToPrint.find(v1);
+      std::map<MVertex*,double>::const_iterator itv2 = _whatToPrint.find(v2);
+      std::map<MVertex*,double>::const_iterator itv3 = _whatToPrint.find(v3);
+      if (!gf){
+	fprintf(f,"ST(%g,%g,%g,%g,%g,%g,%g,%g,%g) {%g,%g,%g};\n",
+		v1->x(),v1->y(),v1->z(),
+		v2->x(),v2->y(),v2->z(),
+		v3->x(),v3->y(),v3->z(),itv1->second,itv2->second,itv3->second);
+      }
+      else {      
+	GPoint p1 = gf->point(SPoint2(v1->x(),v1->y()));
+	GPoint p2 = gf->point(SPoint2(v2->x(),v2->y()));
+	GPoint p3 = gf->point(SPoint2(v3->x(),v3->y()));
+	fprintf(f,"ST(%g,%g,%g,%g,%g,%g,%g,%g,%g) {%g,%g,%g};\n",
+		p1.x(),p1.y(),p1.z(),
+		p2.x(),p2.y(),p2.z(),
+		p3.x(),p3.y(),p3.z(),itv1->second,itv2->second,itv3->second);
+      }
     }
   }
   fprintf(f,"};\n");
diff --git a/Mesh/BackgroundMesh.h b/Mesh/BackgroundMesh.h
index 69526e37ca..a272cc0978 100644
--- a/Mesh/BackgroundMesh.h
+++ b/Mesh/BackgroundMesh.h
@@ -69,18 +69,23 @@ class backgroundMesh : public simpleFunction<double>
   static backgroundMesh *current () { return _current; }
   static void setSizeFactor (double s) {sizeFactor = s;}
   void propagate1dMesh(GFace *);
+  void propagateCrossField(GFace *, simpleFunction<double> *);
+  void propagateCrossFieldHJ(GFace *);
   void propagateCrossField(GFace *);
   void propagateCrossFieldByDistance(GFace *);
   void updateSizes(GFace *);
   double operator () (double u, double v, double w) const; // returns mesh size
   bool inDomain (double u, double v, double w) const; // returns true if in domain
   double getAngle(double u, double v, double w) const ; 
+  double getSmoothness(double u, double v, double w)  ; 
+  double getSmoothness(MElement*) ; 
   void print(const std::string &filename, GFace *gf, 
-              const std::map<MVertex*, double>&) const;
-  void print(const std::string &filename, GFace *gf, int choice = 0) const
+	     const std::map<MVertex*, double>&, int smooth = 0) ;
+  void print(const std::string &filename, GFace *gf, int choice = 0) 
   {
     switch(choice) {
     case 0 : print(filename, gf, _sizes); return;
+    case 2 : print(filename, gf, _sizes, 1); return;
     default : print(filename, gf, _angles); return;
     }
   }
diff --git a/Mesh/meshGFace.cpp b/Mesh/meshGFace.cpp
index 7780c81f61..95248a9d9c 100644
--- a/Mesh/meshGFace.cpp
+++ b/Mesh/meshGFace.cpp
@@ -2122,7 +2122,6 @@ static bool meshGeneratorPeriodic(GFace *gf, bool debug = true)
   {
     /// FIXME FOR PERIODIC : SOME MVERTices SHOULD BE DUPLICATED ...
     /// Still to be done...
-    printf("coucou1\n");
     std::vector<MVertex*> v;
     std::map<MVertex*, BDS_Point*> recoverMapInv;
     for(unsigned int i = 0; i < edgeLoops_BDS.size(); i++){
@@ -2134,7 +2133,7 @@ static bool meshGeneratorPeriodic(GFace *gf, bool debug = true)
       }
     }
 
-    printf("coucou2 %d verices\n",v.size());
+    //    printf("coucou2 %d verices\n",v.size());
     std::map<MVertex*,SPoint3> pos;
     for(unsigned int i = 0; i < v.size(); i++) {
       MVertex *v0 = v[i];
diff --git a/Mesh/meshGFaceDelaunayInsertion.cpp b/Mesh/meshGFaceDelaunayInsertion.cpp
index ed1e61a4cf..feab271a35 100644
--- a/Mesh/meshGFaceDelaunayInsertion.cpp
+++ b/Mesh/meshGFaceDelaunayInsertion.cpp
@@ -1737,13 +1737,14 @@ void bowyerWatsonParallelograms(GFace *gf,
   int nbSwaps = edgeSwapPass(gf, AllTris, SWCR_DEL, DATA);
   Msg::Debug("Delaunization of the initial mesh done (%d swaps)", nbSwaps);
 
-  std::sort(packed.begin(), packed.end(), MVertexLessThanLexicographic());
+  //std::sort(packed.begin(), packed.end(), MVertexLessThanLexicographic());
+  SortHilbert(packed);
 
   //  printf("staring to insert points\n");
   N_GLOBAL_SEARCH = 0;
   N_SEARCH = 0;
   DT_INSERT_VERTEX = 0;
-  // double t1 = Cpu();
+   double t1 = Cpu();
   MTri3 *oneNewTriangle = 0;
   for (unsigned int i=0;i<packed.size();){
     MTri3 *worst = *AllTris.begin();
@@ -1784,9 +1785,9 @@ void bowyerWatsonParallelograms(GFace *gf,
 
   }
   //  printf("%d vertices \n",(int)packed.size());
-  //clock_t t2 = clock();
-  //double DT = (double)(t2-t1)/CLOCKS_PER_SEC;
-  //if (packed.size())printf("points inserted DT %12.5E points per minut : %12.5E %d global searchs %d seachs per insertion\n",DT,60.*packed.size()/DT,N_GLOBAL_SEARCH,N_SEARCH / packed.size());
+  clock_t t2 = clock();
+  double DT = (double)(t2-t1)/CLOCKS_PER_SEC;
+  if (packed.size())printf("points inserted DT %12.5E points per minut : %12.5E %d global searchs %d seachs per insertion\n",DT,60.*packed.size()/DT,N_GLOBAL_SEARCH,N_SEARCH / packed.size());
   transferDataStructure(gf, AllTris, DATA);
   backgroundMesh::unset();
 #if defined(HAVE_ANN)
diff --git a/Mesh/meshGFaceOptimize.cpp b/Mesh/meshGFaceOptimize.cpp
index 2a958076b6..0181479738 100644
--- a/Mesh/meshGFaceOptimize.cpp
+++ b/Mesh/meshGFaceOptimize.cpp
@@ -3632,11 +3632,12 @@ void recombineIntoQuads(GFace *gf,
 	std::set<MEdge,Less_Edge> prioritory;
 	double exbad = -100;
         while(1){
-	  //          int maxCavitySize = CTX::instance()->mesh.bunin;
+	  //	  int maxCavitySize = CTX::instance()->mesh.bunin;
 	  //	  optistatus[0] = (ITERB == 1) ?splitFlatQuads(gf, .01, prioritory) : 0;
           //optistatus[1] =
 	  removeTwoQuadsNodes(gf);
-	  //optistatus[4] = _defectsRemovalBunin(gf,36);
+	  //optistatus[4] = 
+	  //	  _defectsRemovalBunin(gf,maxCavitySize);
 	  //optistatus[2] =
 	  removeDiamonds(gf) ;
 	  if(haveParam)laplaceSmoothing(gf,CTX::instance()->mesh.nbSmoothing,true);
diff --git a/Mesh/meshRefine.cpp b/Mesh/meshRefine.cpp
index 48c13fe417..3fdb7928d4 100644
--- a/Mesh/meshRefine.cpp
+++ b/Mesh/meshRefine.cpp
@@ -20,6 +20,11 @@
 #include "Context.h"
 #include "meshGFaceOptimize.h"
 
+void subdivide_pyramid(MElement* element,
+		       GRegion* gr, 
+		       faceContainer &faceVertices,
+		       std::vector<MHexahedron*> &dwarfs88);
+
 class MVertexLessThanParam{
  public:
   bool operator()(const MVertex *v1, const MVertex *v2) const
@@ -185,6 +190,7 @@ static void Subdivide(GFace *gf, bool splitIntoQuads, bool splitIntoHexas,
 static void Subdivide(GRegion *gr, bool splitIntoHexas, faceContainer &faceVertices)
 {
   if(!splitIntoHexas){
+    // Split tets into other tets
     std::vector<MTetrahedron*> tetrahedra2;
     for(unsigned int i = 0; i < gr->tetrahedra.size(); i++){
       MTetrahedron *t = gr->tetrahedra[i];
@@ -213,6 +219,7 @@ static void Subdivide(GRegion *gr, bool splitIntoHexas, faceContainer &faceVerti
     gr->tetrahedra = tetrahedra2;
   }
 
+  // Split hexes into other hexes.
   std::vector<MHexahedron*> hexahedra2;
   for(unsigned int i = 0; i < gr->hexahedra.size(); i++){
     MHexahedron *h = gr->hexahedra[i];
@@ -246,6 +253,7 @@ static void Subdivide(GRegion *gr, bool splitIntoHexas, faceContainer &faceVerti
     delete h;
   }
 
+  // Split tets into other hexes.
   if(splitIntoHexas){
     for(unsigned int i = 0; i < gr->tetrahedra.size(); i++){
       MTetrahedron *t = gr->tetrahedra[i];
@@ -332,7 +340,21 @@ static void Subdivide(GRegion *gr, bool splitIntoHexas, faceContainer &faceVerti
       }
     }
     gr->prisms.clear();
-
+    // --------------------------------------------------
+    // YAMAKAWA DIVISION OF A PYRAMID INTO 88 HEXES !!!!!
+    // --------------------------------------------------
+    std::vector<MHexahedron*> dwarfs88;
+    for(unsigned int i = 0; i < gr->pyramids.size(); i++){
+      MPyramid *p = gr->pyramids[i];
+      if(p->getNumVertices() == 14){
+	subdivide_pyramid(p,gr,faceVertices,dwarfs88);
+	for (int j=0;j<88;j++)hexahedra2.push_back(dwarfs88[j]);
+      }
+    }
+    gr->pyramids.clear();
+    // --------------------------------------------------
+    // ---- THANKS TO TRISTAN CARRIER BAUDOUIN ----------
+    // --------------------------------------------------
   }
   gr->hexahedra = hexahedra2;
 
@@ -456,3 +478,560 @@ void RefineMesh(GModel *m, bool linear, bool splitIntoQuads, bool splitIntoHexas
   double t2 = Cpu();
   Msg::StatusBar(true, "Done refining mesh (%g s)", t2 - t1);
 }
+
+
+///------ Tristan Carrier Baudouin's Contribution on Full Hex Meshing
+
+static double schneiders_x(int i){
+  double coord[105] = {
+    0.500000,
+    0.666667,
+    0.500000,
+    1.000000,
+    1.000000,
+    1.000000,
+    0.289057,
+    0.324970,
+    0.276710,
+    0.337200,
+    0.364878,
+    0.325197,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    -0.289057,
+    -0.324970,
+    -0.276710,
+    -0.337200,
+    -0.364878,
+    -0.325197,
+    -0.500000,
+    -0.666667,
+    -0.500000,
+    -1.000000,
+    -1.000000,
+    -1.000000,
+    0.084599,
+    0.263953,
+    0.442960,
+    0.310954,
+    0.000000,
+    0.000000,
+    0.118244,
+    0.212082,
+    0.244049,
+    0.213940,
+    0.040495,
+    0.110306,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    -0.118244,
+    -0.212082,
+    -0.244049,
+    -0.213940,
+    -0.040495,
+    -0.110306,
+    -0.084599,
+    -0.263953,
+    -0.442960,
+    -0.310954,
+    0.650493,
+    0.454537,
+    0.000000,
+    0.000000,
+    0.320508,
+    0.264129,
+    0.063695,
+    0.092212,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    -0.320508,
+    -0.264129,
+    -0.063695,
+    -0.092212,
+    -0.650493,
+    -0.454537,
+    0.619616,
+    0.000000,
+    0.277170,
+    0.124682,
+    0.000000,
+    0.000000,
+    -0.277170,
+    -0.124682,
+    -0.619616,
+    0.128101,
+    0.000000,
+    0.176104,
+    0.084236,
+    0.000000,
+    0.000000,
+    -0.176104,
+    -0.084236,
+    -0.128101,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.000000
+  };
+
+  return coord[i];
+}
+
+static double schneiders_y(int i){
+  double coord[105] = {
+    0.707107,
+    0.471405,
+    0.707107,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.474601,
+    0.421483,
+    0.463847,
+    0.367090,
+    0.344843,
+    0.361229,
+    0.429392,
+    0.410339,
+    0.435001,
+    0.407674,
+    0.401208,
+    0.404164,
+    0.474601,
+    0.421483,
+    0.463847,
+    0.367090,
+    0.344843,
+    0.361229,
+    0.707107,
+    0.471405,
+    0.707107,
+    0.000000,
+    0.000000,
+    0.000000,
+    0.536392,
+    0.697790,
+    0.569124,
+    0.742881,
+    0.558045,
+    0.946690,
+    0.497378,
+    0.532513,
+    0.500133,
+    0.541479,
+    0.530503,
+    0.666252,
+    0.463274,
+    0.465454,
+    0.430972,
+    0.451135,
+    0.515274,
+    0.589713,
+    0.497378,
+    0.532513,
+    0.500133,
+    0.541479,
+    0.530503,
+    0.666252,
+    0.536392,
+    0.697790,
+    0.569124,
+    0.742881,
+    0.080018,
+    0.104977,
+    0.216381,
+    0.200920,
+    0.326416,
+    0.339933,
+    0.280915,
+    0.305725,
+    0.396502,
+    0.394423,
+    0.310617,
+    0.337499,
+    0.326416,
+    0.339933,
+    0.280915,
+    0.305725,
+    0.080018,
+    0.104977,
+    0.081443,
+    0.204690,
+    0.354750,
+    0.334964,
+    0.403611,
+    0.367496,
+    0.354750,
+    0.334964,
+    0.081443,
+    0.501199,
+    0.538575,
+    0.447454,
+    0.486224,
+    0.431723,
+    0.470065,
+    0.447454,
+    0.486224,
+    0.501199,
+    0.488701,
+    0.471405,
+    0.017336,
+    0.000000,
+    0.452197,
+    0.471405,
+    0.057682,
+    0.000000,
+    1.414213,
+    0.015731,
+    0.000000
+  };
+
+  return coord[i];
+}
+
+static double schneiders_z(int i){
+  double coord[105] = {
+    0.500000,
+    0.000000,
+    -0.500000,
+    1.000000,
+    0.000000, 
+    -1.000000,
+    0.051666,
+    -0.058015,
+    -0.148140,
+    0.071987,
+    -0.057896,
+    -0.181788,
+    -0.016801,
+    -0.054195,
+    -0.104114,
+    -0.015276,
+    -0.054392,
+    -0.110605,
+    0.051666,
+    -0.058015,
+    -0.148140,
+    0.071987,
+    -0.057896,
+    -0.181788,
+    0.500000,
+    0.000000,
+    -0.500000,
+    1.000000,
+    0.000000,
+    -1.000000,
+    -0.208673,
+    -0.162945,
+    0.021476,
+    0.389516,
+    -0.157646,
+    0.159885,
+    -0.142645,
+    -0.073557,
+    -0.032793,
+    0.060339,
+    -0.136482,
+    0.043449,
+    -0.111103,
+    -0.079664,
+    -0.047879,
+    -0.008734,
+    -0.124554,
+    0.008560,
+    -0.142645,
+    -0.073557,
+    -0.032793,
+    0.060339,
+    -0.136482,
+    0.043449,
+    -0.208673,
+    -0.162945,
+    0.021476,
+    0.389516,
+    -0.041899,
+    -0.680880,
+    -0.103504,
+    -0.392255,
+    -0.065989,
+    -0.212535,
+    -0.093142,
+    -0.227139,
+    -0.056201,
+    -0.124443,
+    -0.087185,
+    -0.182164,
+    -0.065989,
+    -0.212535,
+    -0.093142,
+    -0.227139,
+    -0.041899,
+    -0.680880,
+    0.786284,
+    0.443271,
+    0.104202,
+    0.144731,
+    -0.005330,
+    0.073926,
+    0.104202,
+    0.144731,
+    0.786284,
+    -0.364254,
+    -0.282882,
+    -0.189794,
+    -0.182143,
+    -0.127036,
+    -0.148665,
+    -0.189794,
+    -0.182143,
+    -0.364254,
+    0.642222,
+    0.666667,
+    0.959658,
+    1.000000,
+    -0.455079,
+    -0.666667,
+    -0.844452,
+    -1.000000,
+    0.000000,
+    -0.009020,
+    0.000000
+  };
+
+  return coord[i];
+}
+
+static int schneiders_connect(int i,int j){
+  int n0[88] = {
+    0,1,6,7,12,13,18,19,41,39,37,
+    41,36,40,47,45,43,47,42,46,53,51,
+    49,53,48,52,35,57,55,35,54,34,34,
+    35,65,63,64,62,69,67,68,66,73,71,
+    72,70,61,75,60,74,60,61,79,78,81,
+    80,83,82,77,84,77,88,87,90,89,92,
+    91,86,93,86,57,24,95,85,2,99,84,
+    74,97,104,104,97,26,35,35,24,35,24
+  };
+
+  int n1[88] = {
+    1,2,7,8,13,14,19,20,39,6,38,
+    37,37,36,45,12,44,43,43,42,51,18,
+    50,49,49,48,57,24,56,55,55,54,40,
+    41,63,11,62,10,67,17,66,16,71,23,
+    70,22,75,29,74,28,64,65,78,9,80,
+    15,82,21,84,27,79,87,8,89,14,91,
+    20,93,26,88,35,57,94,86,85,98,77,
+    60,96,103,28,27,99,55,102,25,31,102
+  };
+
+  int n2[88] = {
+    4,5,10,11,16,17,22,23,38,7,7,
+    36,8,87,44,13,13,42,14,89,50,19,
+    19,48,20,91,56,25,25,54,26,93,46,
+    47,62,10,78,9,66,16,80,15,70,22,
+    82,21,74,28,84,27,68,69,39,6,45,
+    12,51,18,57,24,81,63,11,67,17,71,
+    23,75,29,90,33,94,33,93,98,93,76,
+    58,76,58,74,84,2,26,2,26,2,0
+  };
+
+  int n3[88] = {
+    3,4,9,10,15,16,21,22,37,38,8,
+    40,87,88,43,44,14,46,89,90,49,50,
+    20,52,91,92,55,56,26,34,93,86,52,
+    53,64,62,79,78,68,66,81,80,72,70,
+    83,82,60,74,77,84,72,73,41,39,47,
+    45,53,51,35,57,83,65,63,69,67,73,
+    71,61,75,92,94,95,0,98,99,26,96,
+    103,3,4,103,96,102,102,31,102,102,95
+  };
+
+  int n4[88] = {
+    6,7,12,13,18,19,24,25,35,33,31,
+    35,30,34,41,39,37,41,36,40,47,45,
+    43,47,42,46,53,51,49,53,48,52,86,
+    34,61,59,60,58,65,63,64,62,69,67,
+    68,66,73,71,72,70,77,60,77,76,79,
+    78,81,80,83,82,35,86,85,88,87,90,
+    89,92,91,61,84,27,97,59,5,101,74,
+    75,104,101,101,104,93,34,34,57,33,57
+  };
+
+  int n5[88] = {
+    7,8,13,14,19,20,25,26,33,0,32,
+    31,31,30,39,6,38,37,37,36,45,12,
+    44,43,43,42,51,18,50,49,49,48,88,
+    40,59,5,58,4,63,11,62,10,67,17,
+    66,16,71,23,70,22,79,64,76,3,78,
+    9,80,15,82,21,41,85,2,87,8,89,
+    14,91,20,65,77,84,96,61,59,100,60,
+    61,103,100,29,28,98,54,86,56,32,35
+  };
+
+  int n6[88] = {
+    10,11,16,17,22,23,28,29,32,1,1,
+    30,2,85,38,7,7,36,8,87,44,13,
+    13,42,14,89,50,19,19,48,20,91,90,
+    46,58,4,76,3,62,10,78,9,66,16,
+    80,15,70,22,82,21,81,68,33,0,39,
+    6,45,12,51,18,47,59,5,63,11,67,
+    17,71,23,69,76,96,76,75,100,75,58,
+    59,58,59,75,74,85,93,85,55,1,33
+  };
+
+  int n7[88] = {
+    9,10,15,16,21,22,27,28,31,32,2,
+    34,85,86,37,38,8,40,87,88,43,44,
+    14,46,89,90,49,50,20,52,91,92,92,
+    52,60,58,77,76,64,62,79,78,68,66,
+    81,80,72,70,83,82,83,72,35,33,41,
+    39,47,45,53,51,53,61,59,65,63,69,
+    67,73,71,73,96,97,3,100,101,29,103,
+    100,4,5,100,103,86,86,30,35,0,94
+  };
+
+  if(i==0){
+    return n0[j];
+  }
+  else if(i==1){
+    return n1[j];
+  }
+  else if(i==2){
+    return n2[j];
+  }
+  else if(i==3){
+    return n3[j];
+  }
+  else if(i==4){
+    return n4[j];
+  }
+  else if(i==5){
+    return n5[j];
+  }
+  else if(i==6){
+    return n6[j];
+  }
+  else{
+    return n7[j];
+  }
+}
+
+
+void subdivide_pyramid(MElement* element,
+		       GRegion* gr, 
+		       faceContainer &faceVertices,
+		       std::vector<MHexahedron*> &dwarfs88)
+{
+  unsigned int i;
+  int index1,index2,index3,index4;
+  int index5,index6,index7,index8;
+  SPoint3 point;
+  std::vector<MVertex*> v;
+
+  dwarfs88.resize(88);
+  v.resize(105);
+  
+  for (int i=0;i<105;i++)v[i] = NULL;
+  
+  v[29] = element->getVertex(0);
+  v[27] = element->getVertex(1);
+  v[3] = element->getVertex(2);
+  v[5] = element->getVertex(3);
+  v[102] = element->getVertex(4);
+
+  v[28] = element->getVertex(5);
+  v[97] = element->getVertex(8);
+  v[4] = element->getVertex(10);
+  v[101] = element->getVertex(6);
+  v[26] = element->getVertex(7);
+  v[24] = element->getVertex(9);
+  v[0] = element->getVertex(11);
+  v[2] = element->getVertex(12);
+
+  // the one in the middle of rect face
+  v[104] = element->getVertex(13);
+
+  faceContainer::iterator fIter;
+
+  fIter = faceVertices.find(MFace(v[29],v[27],v[102]));
+  if (fIter != faceVertices.end())
+    v[25] = fIter->second[0];
+  else {
+    element->pnt(0.0,-0.666667,0.471405/1.414213,point);
+    v[25] = new MVertex(point.x(),point.y(),point.z(),gr);
+    gr->addMeshVertex(v[25]);
+    faceVertices[MFace(v[29],v[27],v[102])].push_back(v[25]);
+  }
+  
+  fIter = faceVertices.find(MFace(v[27],v[3],v[102]));
+  if (fIter != faceVertices.end())
+    v[95] = fIter->second[0];
+  else {
+    element->pnt(0.666667,0.0,0.471405/1.414213,point);
+    v[95] = new MVertex(point.x(),point.y(),point.z(),gr);
+    gr->addMeshVertex(v[95]);
+    faceVertices[MFace(v[27],v[3],v[102])].push_back(v[95]);
+  }
+
+  fIter = faceVertices.find(MFace(v[3],v[5],v[102]));
+  if (fIter != faceVertices.end())
+    v[1] = fIter->second[0];
+  else {
+    element->pnt(0.0,0.666667,0.471405/1.414213,point);
+    v[1] = new MVertex(point.x(),point.y(),point.z(),gr);
+    gr->addMeshVertex(v[1]);
+    faceVertices[MFace(v[3],v[5],v[102])].push_back(v[1]);
+  }
+
+  fIter = faceVertices.find(MFace(v[5],v[29],v[102]));
+  if (fIter != faceVertices.end())
+    v[99] = fIter->second[0];
+  else {
+    element->pnt(-0.666667,0.0,0.471405/1.414213,point);
+    v[99] = new MVertex(point.x(),point.y(),point.z(),gr);
+    gr->addMeshVertex(v[99]);
+    faceVertices[MFace(v[5],v[29],v[102])].push_back(v[99]);
+  }
+  
+  for(i=0;i<105;i++){
+    if (!v[i]){
+      element->pnt(schneiders_z(i),schneiders_x(i),schneiders_y(i)/1.414213,point);
+      v[i] = new MVertex(point.x(),point.y(),point.z(),gr);
+      gr->addMeshVertex(v[i]);
+    }
+  }
+
+  for(i=0;i<88;i++){
+    index1 = schneiders_connect(0,i);
+    index2 = schneiders_connect(1,i);
+    index3 = schneiders_connect(2,i);
+    index4 = schneiders_connect(3,i);
+    index5 = schneiders_connect(4,i);
+    index6 = schneiders_connect(5,i);
+    index7 = schneiders_connect(6,i);
+    index8 = schneiders_connect(7,i);
+    
+    dwarfs88[i]=(new MHexahedron(v[index1],v[index2],
+				v[index3],v[index4],
+				v[index5],v[index6],
+				v[index7],v[index8]));
+  }
+}
+
diff --git a/Mesh/surfaceFiller.cpp b/Mesh/surfaceFiller.cpp
index a6d8d81ef2..9d4361d414 100644
--- a/Mesh/surfaceFiller.cpp
+++ b/Mesh/surfaceFiller.cpp
@@ -54,6 +54,7 @@ struct surfacePointWithExclusionRegion {
   SPoint2 _q[4];
   SMetric3 _meshMetric;
   double _distanceSummed;
+  double mat1[2][2], mat2[2][2];
   /*
          + p3
     p4   |
@@ -80,6 +81,10 @@ struct surfacePointWithExclusionRegion {
     else {
       _distanceSummed = father->_distanceSummed + distance (father->_v,_v);
     }
+    //test !!!
+    //    const double s = backgroundMesh::current()->getSmoothness(_mp.x(),_mp.y(),0);
+    //    if(s  > 0.02) 
+    //      _distanceSummed = 10000*s;
   }
   bool inExclusionZone (const SPoint2 &p){
     double mat[2][2];
@@ -180,7 +185,7 @@ bool inExclusionZone (SPoint2 &p,
   if (!backgroundMesh::current()->inDomain(p.x(),p.y(),0)) return true;
 
   my_wrapper w (p);
-  double _min[2] = {p.x()-1.e-1, p.y()-1.e-1},_max[2] = {p.x()+1.e-1,p.y()+1.e-1};
+  double _min[2] = {p.x()-1.e-8, p.y()-1.e-8},_max[2] = {p.x()+1.e-8,p.y()+1.e-8};
   rtree.Search(_min,_max,rtree_callback,&w);
 
   return w._tooclose;
@@ -968,7 +973,6 @@ void packingOfParallelograms(GFace* gf,  std::vector<MVertex*> &packed, std::vec
 
   //  printf("initially : %d vertices in the domain\n",vertices.size());
 
-
   while(!fifo.empty()){
     //surfacePointWithExclusionRegion & parent = fifo.top();
     //    surfacePointWithExclusionRegion * parent = fifo.front();
diff --git a/Numeric/CMakeLists.txt b/Numeric/CMakeLists.txt
index 0cc2843650..e4449b7522 100644
--- a/Numeric/CMakeLists.txt
+++ b/Numeric/CMakeLists.txt
@@ -37,6 +37,8 @@ set(SRC
  decasteljau.cpp
   mathEvaluator.cpp
   Iso.cpp
+  approximationError.cpp
+  ConjugateGradients.cpp
 )
 
 file(GLOB HDR RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.h) 
diff --git a/Numeric/JacobianBasis.cpp b/Numeric/JacobianBasis.cpp
index cb784333e1..fd11c417d9 100644
--- a/Numeric/JacobianBasis.cpp
+++ b/Numeric/JacobianBasis.cpp
@@ -843,7 +843,8 @@ int JacobianBasis::jacobianOrder(int parentType, int order)
     case TYPE_LIN : return order - 1;
     case TYPE_TRI : return 2*order - 2;
     case TYPE_QUA : return 2*order - 1;
-    case TYPE_TET : return 3*order - 3;
+      //    case TYPE_TET : return 3*order - 3;
+    case TYPE_TET : return order;
     case TYPE_PRI : return 3*order - 1;
     case TYPE_HEX : return 3*order - 1;
     case TYPE_PYR : return 3*order - 3;
diff --git a/Numeric/Numeric.cpp b/Numeric/Numeric.cpp
index aba8d5f933..e38ab1b70e 100644
--- a/Numeric/Numeric.cpp
+++ b/Numeric/Numeric.cpp
@@ -702,120 +702,6 @@ bool newton_fd(bool (*func)(fullVector<double> &, fullVector<double> &, void *),
   return false;
 }
 
-/*
-  min_a f(x+a*d);
-  f(x+a*d) = f(x) + f'(x) (
-*/
-
-void gmshLineSearch(double (*func)(fullVector<double> &, void *), void* data,
-                    fullVector<double> &x, fullVector<double> &p,
-                    fullVector<double> &g, double &f,
-                    double stpmax, int &check)
-{
-  int i;
-  double alam, alam2 = 1., alamin, f2 = 0., fold2 = 0., rhs1, rhs2, temp, tmplam = 0.;
-
-  const double ALF = 1.0e-4;
-  const double TOLX = 1.0e-9;
-
-  fullVector<double> xold(x);
-  const double fold = (*func)(xold, data);
-
-  check=0;
-  int n = x.size();
-  double norm = p.norm();
-  if (norm > stpmax) p.scale(stpmax / norm);
-  double slope=0.0;
-  for (i = 0; i < n; i++) slope += g(i)*p(i);
-  double test=0.0;
-  for (i = 0; i < n; i++) {
-    temp = fabs(p(i)) / std::max(fabs(xold(i)), 1.0);
-    if (temp > test) test = temp;
-  }
-  /*
-    for (int j=0;j<100;j++){
-    double sx = (double)j/99;
-    for (i=0;i<n;i++) x(i)=xold(i)+10*sx*p(i);
-    double jzede = (*func)(x,data);
-    }
-  */
-
-  alamin = TOLX / test;
-  alam = 1.0;
-  while(1) {
-    for (i = 0; i < n; i++) x(i) = xold(i) + alam*p(i);
-    f = (*func)(x, data);
-    //    printf("f = %g x = %g %g alam = %g p = %g %g\n",f,x(0),x(1),alam,p(0),p(1));
-    if (alam < alamin) {
-      for (i = 0; i <n; i++) x(i) = xold(i);
-      //      printf("ALERT : alam %g alamin %g\n",alam,alamin);
-      check = 1;
-      return;
-    }
-    else if (f <= fold + ALF * alam * slope) return;
-    else {
-      if (alam == 1.0)
-        tmplam = -slope / (2.0 * (f - fold - slope));
-      else {
-        rhs1 = f - fold - alam * slope;
-        rhs2 = f2 - fold2 - alam2 * slope;
-        const double a = (rhs1/(alam*alam)-rhs2/(alam2*alam2))/(alam-alam2);
-        const double b = (-alam2*rhs1/(alam*alam)+alam*rhs2/(alam2*alam2))/(alam-alam2);
-        if (a == 0.0) tmplam = -slope / (2.0 * b);
-        else {
-          const double disc = b*b-3.0*a*slope;
-          if (disc < 0.0) Msg::Error("Roundoff problem in gmshLineSearch.");
-          else tmplam = (-b+sqrt(disc))/(3.0*a);
-        }
-        if (tmplam > 0.5 * alam)
-          tmplam = 0.5 * alam;
-      }
-    }
-    alam2 = alam;
-    f2 = f;
-    fold2 = fold;
-    alam = std::max(tmplam, 0.1 * alam);
-  }
-}
-
-double minimize_grad_fd(double (*func)(fullVector<double> &, void *),
-                        fullVector<double> &x, void *data)
-{
-  const int MAXIT = 3;
-  const double EPS = 1.e-4;
-  const int N = x.size();
-
-  fullVector<double> grad(N);
-  fullVector<double> dir(N);
-  double f, feps;//, finit;
-
-  for (int iter = 0; iter < MAXIT; iter++){
-    // compute gradient of func
-    f = func(x, data);
-    //if (iter == 0) finit = f;
-    // printf("Opti iter %d x = (%g %g) f = %g\n",iter,x(0),x(1),f);
-    // printf("grad = (");
-    for (int j = 0; j < N; j++){
-      double h = EPS * fabs(x(j));
-      if(h == 0.) h = EPS;
-      x(j) += h;
-      feps = func(x, data);
-      grad(j) = (feps - f) / h;
-      // printf("%g ",grad(j));
-      dir(j) = -grad(j);
-      x(j) -= h;
-    }
-    // printf(")\n ");
-    // do a 1D line search to fine the minimum
-    // of f(x - \alpha \nabla f)
-    double f, stpmax=100000;
-    int check;
-    gmshLineSearch(func, data, x, dir, grad, f, stpmax, check);
-    // printf("Line search done x = (%g %g) f = %g\n",x(0),x(1),f);
-    if (check == 1) break;
-  }
-  return f;
-}
 
 /*
   P(p) = p1 + t1 xi + t2 eta
@@ -1404,8 +1290,8 @@ void signedDistancesPointsEllipseLine(std::vector<double>&distances,
   }
 }
 
-int intersection_segments(const SPoint3 &p1, const SPoint3 &p2,
-                          const SPoint3 &q1, const SPoint3 &q2,
+int intersection_segments(const SPoint2 &p1, const SPoint2 &p2,
+                          const SPoint2 &q1, const SPoint2 &q2,
                           double x[2])
 {
   double xp_max = std::max(p1.x(), p2.x());
@@ -1433,6 +1319,64 @@ int intersection_segments(const SPoint3 &p1, const SPoint3 &p2,
             x[1] >= 0.0 && x[1] <= 1.);
   }
 }
+/// 3D VERSION
+int intersection_segments(const SPoint3 &p1, const SPoint3 &p2,
+                          const SPoint3 &q1, const SPoint3 &q2,
+                          double x[2])
+{
+  SVector3 v1(p2,p1),v2(q2,q1);
+  double n1 = v1.norm();
+  double n2 = v2.norm();
+  double EPS = 1.e-10*std::max(n1,n2);
+  double A[2][2];
+  A[0][0] = p2.x() - p1.x();
+  A[0][1] = q1.x() - q2.x();
+  A[1][0] = p2.y() - p1.y();
+  A[1][1] = q1.y() - q2.y();
+  double a[2] = {q1.x() - p1.x(), q1.y() - p1.y()};
+  double B[2][2];
+  B[0][0] = p2.z() - p1.z();
+  B[0][1] = q1.z() - q2.z();
+  B[1][0] = p2.y() - p1.y();
+  B[1][1] = q1.y() - q2.y();
+  double b[2] = {q1.z() - p1.z(), q1.y() - p1.y()};
+  double C[2][2];
+  C[0][0] = p2.z() - p1.z();
+  C[0][1] = q1.z() - q2.z();
+  C[1][0] = p2.x() - p1.x();
+  C[1][1] = q1.x() - q2.x();
+  double c[2] = {q1.z() - p1.z(), q1.x() - p1.x()};
+  double detA = fabs(det2x2(A));
+  double detB = fabs(det2x2(B));
+  double detC = fabs(det2x2(C));
+  //  printf("%12.5E %12.5E %12.5E\n",detA,detB,detC);
+  if (detA > detB && detA > detC)
+    sys2x2(A, a, x);
+  else if (detB > detA && detB > detC)
+    sys2x2(B, b, x);
+  else
+    sys2x2(C, c, x);
+  if(x[0] >= 0.0 && x[0] <= 1. &&
+     x[1] >= 0.0 && x[1] <= 1.) {
+
+    SPoint3 x1 (p1.x()*(1.-x[0]) + p2.x()*x[0],
+		p1.y()*(1.-x[0]) + p2.y()*x[0],
+		p1.z()*(1.-x[0]) + p2.z()*x[0]);
+    SPoint3 x2 (q1.x()*(1.-x[0]) + q2.x()*x[0],
+		q1.y()*(1.-x[0]) + q2.y()*x[0],
+		q1.z()*(1.-x[0]) + q2.z()*x[0]);
+
+    SVector3 d (x2,x1);
+    double nd = norm(d);
+    if (nd > EPS){
+      x[0] = x[1] = 1.e22;
+      return false;
+    }
+    return true;
+  }  
+  return false;
+}
+
 
 void computeMeanPlaneSimple(const std::vector<SPoint3> &points, mean_plane &meanPlane)
 {
diff --git a/Numeric/Numeric.h b/Numeric/Numeric.h
index 00a9b80c65..6535a9783e 100644
--- a/Numeric/Numeric.h
+++ b/Numeric/Numeric.h
@@ -9,6 +9,7 @@
 #include <math.h>
 #include <vector>
 #include "fullMatrix.h"
+#include "SPoint2.h"
 #include "SPoint3.h"
 #include "SVector3.h"
 
@@ -108,8 +109,6 @@ double ComputeScalarRep(int numComp, double *val);
 void invert_singular_matrix3x3(double MM[3][3], double II[3][3]);
 bool newton_fd(bool (*func)(fullVector<double> &, fullVector<double> &, void *),
                fullVector<double> &x, void *data, double relax=1., double tolx=1.e-6);
-double minimize_grad_fd(double (*func)(fullVector<double> &, void *),
-                        fullVector<double> &x, void *data);
 
 void signedDistancePointTriangle(const SPoint3 &p1,const SPoint3 &p2, const SPoint3 &p3,
 				 const SPoint3 &p, double &d, SPoint3 &closePt);
@@ -156,6 +155,9 @@ void signedDistancesPointsEllipseLine (std::vector<double>&distances,
 int intersection_segments (const SPoint3 &p1, const SPoint3 &p2,
 			   const SPoint3 &q1, const SPoint3 &q2,
 			   double x[2]);
+int intersection_segments (const SPoint2 &p1, const SPoint2 &p2,
+			   const SPoint2 &q1, const SPoint2 &q2,
+			   double x[2]);
 
 //tools for projection onto plane
 void computeMeanPlaneSimple(const std::vector<SPoint3> &points, mean_plane &meanPlane);
diff --git a/Numeric/simpleFunction.h b/Numeric/simpleFunction.h
index 10503cce9f..fc93def375 100644
--- a/Numeric/simpleFunction.h
+++ b/Numeric/simpleFunction.h
@@ -19,6 +19,7 @@ class simpleFunction {
   virtual ~simpleFunction(){}
   virtual bool hasDerivatives() {return _hasDerivatives;};
   virtual scalar operator () (double x, double y, double z) const { return _val; }
+  virtual void setElement(MElement *e) const {}
   virtual void gradient (double x, double y, double z,
 			 scalar & dfdx, scalar & dfdy, scalar & dfdz) const
   { dfdx = dfdy = dfdz = 0.0; }
@@ -31,14 +32,36 @@ class simpleFunction {
     dfdzx = dfdzy = dfdzz = 0.0; }
 };
 
+template <class scalar>
+class constantPerElement : public simpleFunction<scalar>
+{
+  std::map<MElement *,scalar> _data;
+  mutable MElement *_e;
+ public :
+  constantPerElement () : _e(0){}
+  void set(MElement *e, scalar v) {
+    _data[e] = v;
+  } 
+  void setElement(MElement *e) const { 
+    _e = e; }
+  virtual scalar operator () (double x, double y, double z) const 
+  { 
+    if (!_e)return 0.0;
+    typename std::map<MElement *,scalar>::const_iterator it = _data.find(_e);
+    if (it == _data.end())return 0.0;
+    return it->second;
+  }
+};
+
+
 template <class scalar>
 class simpleFunctionOnElement : public simpleFunction<scalar>
 {
-  MElement *_e;
+  mutable MElement *_e;
  public :
   simpleFunctionOnElement(scalar val=0) : simpleFunction<scalar>(val),_e(0) {}
   virtual ~simpleFunctionOnElement(){}
-  void setElement(MElement *e) { _e = e; }
+  void setElement(MElement *e) const { _e = e; }
   MElement * getElement(void) const { return _e; }
   MElement * getElement(double x, double y, double z) const
   {
diff --git a/Solver/helmholtzTerm.h b/Solver/helmholtzTerm.h
index 890e1802ab..12a0f0bf19 100644
--- a/Solver/helmholtzTerm.h
+++ b/Solver/helmholtzTerm.h
@@ -47,8 +47,9 @@ class helmholtzTerm : public femTerm<scalar> {
   }
   virtual void elementMatrix(SElement *se, fullMatrix<scalar> &m) const
   {
-
     MElement *e = se->getMeshElement();
+    if (_k)_k->setElement(e);
+    if (_a)_a->setElement(e);
     // compute integration rule
     // const int integrationOrder = (_a) ? 2 * e->getPolynomialOrder() : 
     //2 * (e->getPolynomialOrder() - 1);
diff --git a/benchmarks/2d/brombo2.geo b/benchmarks/2d/brombo2.geo
index a90b025263..a9b7c7725f 100644
--- a/benchmarks/2d/brombo2.geo
+++ b/benchmarks/2d/brombo2.geo
@@ -1,3 +1,4 @@
+//Mesh.RecombinationAlgorithm = 2;
 Point(1) = {0.0325, -0.13, 0, 0.0005};
 Point(2) = {0.0375, -0.13, 0, 0.0005};
 Point(3) = {0.0375, -0.09, 0, 0.0005};
@@ -186,3 +187,5 @@ Physical Surface (1002) = {6, 18, 30};
 Physical Surface (1003) = {12, 24, 36};
 Physical Surface (1004) = {111};
 Physical Surface (1005) = {48, 49};
+Recombine Surface {111};
+Recombine Surface {61, 6, 12, 18, 24, 30, 36, 48, 49, 55, 67, 73, 79, 85, 91, 97, 103, 109, 111};
diff --git a/benchmarks/2d/machine2/Machine.geo b/benchmarks/2d/machine2/Machine.geo
index 9e6b948cc4..88ae4e8491 100644
--- a/benchmarks/2d/machine2/Machine.geo
+++ b/benchmarks/2d/machine2/Machine.geo
@@ -13,6 +13,6 @@ EndIf
 
 Mesh.CharacteristicLengthFactor = 0.3*1.*0.5*2*0.6;
 Mesh.Smoothing = 5;
-Mesh.Algorithm = 1;
+//Mesh.Algorithm = 1;
 
 Coherence;
diff --git a/benchmarks/2d/many_holes.geo b/benchmarks/2d/many_holes.geo
index 1708e2468e..74be409949 100644
--- a/benchmarks/2d/many_holes.geo
+++ b/benchmarks/2d/many_holes.geo
@@ -48,3 +48,4 @@ Plane Surface (20) = {10,11:(10+K)};
 
 //Physical Surface (30) = {20};
 
+Recombine Surface {20};
diff --git a/benchmarks/3d/Cube-01.geo b/benchmarks/3d/Cube-01.geo
index 747fff6673..7e32681db1 100644
--- a/benchmarks/3d/Cube-01.geo
+++ b/benchmarks/3d/Cube-01.geo
@@ -1,8 +1,12 @@
+Mesh.Algorithm3D = 9;
+Mesh.Algorithm = 9;
+Mesh.Recombine3DAll = 1;
+Mesh.Smoothing=1;
 //Mesh.Dual = 1;
 //Mesh.Voronoi=1;
 
-lc = 0.08;
-Point(1) = {0.0,0.0,0.0,lc};         
+lc = 0.16;
+Point(1) = {0.0,0.0,0.0,lc/2};         
 Point(2) = {1,0.0,0.0,lc};         
 Point(3) = {1,1,0.0,lc};         
 Point(4) = {0,1,0.0,lc};         
diff --git a/contrib/HighOrderMeshOptimizer/OptHOM.cpp b/contrib/HighOrderMeshOptimizer/OptHOM.cpp
index fd0e6d1fdd..6efe7eb31a 100644
--- a/contrib/HighOrderMeshOptimizer/OptHOM.cpp
+++ b/contrib/HighOrderMeshOptimizer/OptHOM.cpp
@@ -27,6 +27,8 @@
 //
 // Contributors: Thomas Toulorge, Jonathan Lambrechts
 
+static int NEVAL = 0;
+
 #include <iostream>
 #include <algorithm>
 #include "OptHomMesh.h"
@@ -34,6 +36,10 @@
 #include "OptHomRun.h"
 #include "GmshMessage.h"
 #include "GmshConfig.h"
+#include "ConjugateGradients.h"
+#include "MLine.h"
+#include "MTriangle.h"
+#include "GModel.h"
 
 #if defined(HAVE_BFGS)
 
@@ -106,7 +112,457 @@ bool OptHOM::addJacObjGrad(double &Obj, alglib::real_1d_array &gradObj)
   return true;
 }
 
+bool OptHOM::addJacObjGrad(double &Obj, std::vector<double> &gradObj)
+{
+  minJac = 1.e300;
+  maxJac = -1.e300;
+
+  for (int iEl = 0; iEl < mesh.nEl(); iEl++) {
+    // Scaled Jacobians
+    std::vector<double> sJ(mesh.nBezEl(iEl));
+    // Gradients of scaled Jacobians
+    std::vector<double> gSJ(mesh.nBezEl(iEl)*mesh.nPCEl(iEl));
+    mesh.scaledJacAndGradients (iEl,sJ,gSJ);
+
+    for (int l = 0; l < mesh.nBezEl(iEl); l++) {
+      double f1 = compute_f1(sJ[l], jacBar);
+      Obj += compute_f(sJ[l], jacBar);
+      if (_optimizeBarrierMax) {
+        Obj += compute_f(sJ[l], barrier_min);
+        f1 += compute_f1(sJ[l], barrier_min);
+      }
+      for (int iPC = 0; iPC < mesh.nPCEl(iEl); iPC++)
+        gradObj[mesh.indPCEl(iEl,iPC)] += f1*gSJ[mesh.indGSJ(iEl,l,iPC)];
+      minJac = std::min(minJac, sJ[l]);
+      maxJac = std::max(maxJac, sJ[l]);
+    }
+  }
+
+  return true;
+}
+
+
+bool OptHOM::addApproximationErrorObjGrad(double factor, double &Obj, alglib::real_1d_array &gradObj, simpleFunction<double>& fct)
+{
+  std::vector<double> gradF;
+  for (int iEl = 0; iEl < mesh.nEl(); iEl++) {
+    double f;
+    mesh.approximationErrorAndGradients(iEl, f, gradF, 1.e-6, fct);
+    Obj += f * factor;
+    for (size_t i = 0; i < mesh.nPCEl(iEl); ++i){
+      gradObj[mesh.indPCEl(iEl, i)] += gradF[i] * factor;
+    }
+  }
+  //  printf("DIST = %12.5E\n",DISTANCE);
+  return true;
+
+}
+
+static void computeGradSFAtNodes (MElement *e, std::vector<std::vector<SVector3> > &gsf)
+{
+  const nodalBasis &elbasis = *e->getFunctionSpace();
+  double s[256][3];
+  for (int j=0;j<e->getNumVertices();j++){
+    std::vector<SVector3> g(e->getNumVertices());
+    double u_mesh = elbasis.points(j,0);
+    double v_mesh = elbasis.points(j,1);
+    e->getGradShapeFunctions ( u_mesh , v_mesh , 0,  s);
+    for (int k=0;k<e->getNumVertices();k++)
+      g[k] = SVector3(s[k][0],s[k][1],s[k][2]);
+    gsf.push_back(g);
+  }  
+}
+
+static double MFaceGFaceDistance (MTriangle *t, GFace *gf,  std::vector<std::vector<SVector3> > *gsfT=0,  std::map<MVertex*,SVector3> *normalsToCAD=0) {
+  const double h = t->maxEdge();
+  double jac[3][3];
+  double distFace = 0.0;
+  //  for (int j=0;j<3;j++){
+  for (int j=0;j<t->getNumVertices();j++){
+    // get parametric coordinates of jth vertex
+    // the last line of the jacobian is the normal
+    // to the element @ (u_mesh,v_mesh)
+
+    if (gsfT){
+      double detJ = t->getJacobian((*gsfT)[j],jac);
+    }
+    else{
+      const nodalBasis &elbasis = *t->getFunctionSpace();
+      double u_mesh = elbasis.points(j,0);
+      double v_mesh = elbasis.points(j,1);
+      double detJ = t->getJacobian(u_mesh,v_mesh,0,jac);
+    }
+ 
+    SVector3 tg_mesh (jac[2][0],jac[2][1],jac[2][2]);
+    tg_mesh.normalize();
+
+    SVector3 tg_cad ;
+    if (normalsToCAD)tg_cad = (*normalsToCAD)[t->getVertex(j)]; 
+    else {
+      SPoint2 p_cad;
+      reparamMeshVertexOnFace(t->getVertex (j), gf, p_cad);
+      tg_cad = gf->normal(p_cad);
+      tg_cad.normalize();
+    }
+    SVector3 diff1 = (dot(tg_cad, tg_mesh) > 0) ? 
+      tg_cad - tg_mesh : tg_cad + tg_mesh;
+    //    printf("%g %g %g vs %g %g %g\n",tg_cad.x(),tg_cad.y(),tg_cad.z(),tg_mesh.x(),tg_mesh.y(),tg_mesh.z());
+    distFace += diff1.norm();
+  }
+  return distFace;
+}
+
+static double MLineGEdgeDistance (MLine *l, GEdge *ge, FILE *f = 0) {
+  const nodalBasis &elbasis = *l->getFunctionSpace();
+  const double h = .25*0.5*distance (l->getVertex (0), l->getVertex (1) ) / (l->getNumVertices()-1);
+  double jac[3][3];
+  double distEdge = 0.0;
+
+  //  if(f)printf("%d\n",l->getNumVertices());
+
+  for (int j=0;j<l->getNumVertices();j++){
+    double t_mesh = elbasis.points(j,0);
+    //    if (f) printf("%g ",t_mesh);
+    double detJ = l->getJacobian(t_mesh,0,0,jac);
+    SVector3 tg_mesh (jac[0][0],jac[0][1],jac[0][2]);
+    tg_mesh.normalize();
+    double t_cad;
+    reparamMeshVertexOnEdge(l->getVertex (j), ge, t_cad);
+    SVector3 tg_cad = ge->firstDer(t_cad);
+    tg_cad.normalize();
+
+    SVector3 diff1 = (dot(tg_cad, tg_mesh) > 0) ? 
+      tg_cad - tg_mesh : tg_cad + tg_mesh;
+
+    if (f){
+      fprintf (f,"SP(%g,%g,%g){%g};\n",l->getVertex (j)->x(),
+	       l->getVertex (j)->y(),l->getVertex (j)->z(),h*diff1.norm());
+    }
+
+    //    SVector3 n = crossprod(tg_cad,tg_mesh);
+    //    printf("%g %g vs %g %g\n",tg_cad.x(),tg_cad.y(),tg_mesh.x(),tg_mesh.y());
+    distEdge += diff1.norm();
+  }
+  //  if(f)printf("\n");
+  return h*distEdge;
+}
+
+
+void distanceFromElementsToGeometry(GModel *gm, int dim, std::map<MElement*,double> &distances){
+
+  std::map<MEdge,double,Less_Edge> dist2Edge;
+  for (GModel::eiter it = gm->firstEdge(); it != gm->lastEdge(); ++it){
+    if ((*it)->geomType() == GEntity::Line)continue;
+    for (unsigned int i=0;i<(*it)->lines.size(); i++){
+      double d = MLineGEdgeDistance ( (*it)->lines[i] , *it );
+      MEdge e =  (*it)->lines[i]->getEdge(0);
+      dist2Edge[e] = d;
+    }
+  }
+
+  //  printf("DISTANCE TO GEOMETRY : 1D PART %22.15E\n",Obj);
+  
+  std::map<MFace,double,Less_Face> dist2Face;
+  for(GModel::fiter it = gm->firstFace(); it != gm->lastFace(); ++it){
+    if ((*it)->geomType() == GEntity::Plane)continue;
+    for (unsigned int i=0;i<(*it)->triangles.size(); i++){
+      double d = MFaceGFaceDistance ( (*it)->triangles[i] , *it );
+      MFace f =  (*it)->triangles[i]->getFace(0);
+      dist2Face[f] = d;
+    }
+  }
+
+  std::vector<GEntity*> entities;
+  gm->getEntities(entities);
+  for (int iEnt = 0; iEnt < entities.size(); ++iEnt) {
+    GEntity* &entity = entities[iEnt];
+    if (entity->dim() != dim) continue;
+    for (int iEl = 0; iEl < entity->getNumMeshElements();iEl++) {       // Detect bad elements
+      MElement *element = entity->getMeshElement(iEl);
+      double d = 0.0;
+      for (int iEdge = 0; iEdge < element->getNumEdges(); ++iEdge) {
+	MEdge e =  element->getEdge(iEdge);
+	std::map<MEdge,double,Less_Edge>::iterator it = dist2Edge.find(e);
+	if(it != dist2Edge.end())d+=it->second;
+      }
+      for (int iFace = 0; iFace < element->getNumFaces(); ++iFace) {
+	MFace f =  element->getFace(iFace);
+	std::map<MFace,double,Less_Face>::iterator it = dist2Face.find(f);
+	if(it != dist2Face.end())d+=it->second;
+      }
+      distances[element] = d;
+    }
+  }
+}
+
+
+double distanceToGeometry(GModel *gm)
+{
+
+  FILE *f = fopen("toto.pos","w");
+  fprintf(f,"View \"\"{\n");
+
+  double Obj = 0.0;
+ 
+  for (GModel::eiter it = gm->firstEdge(); it != gm->lastEdge(); ++it){
+    if ((*it)->geomType() == GEntity::Line)continue;
+    for (unsigned int i=0;i<(*it)->lines.size(); i++){
+      //Obj += MLineGEdgeDistance ( (*it)->lines[i] , *it,f );
+      Obj = std::max(MLineGEdgeDistance ( (*it)->lines[i] , *it, f ),Obj);
+    }
+  }
+
+    printf("DISTANCE TO GEOMETRY : 1D PART %22.15E\n",Obj);
+  
+  for(GModel::fiter it = gm->firstFace(); it != gm->lastFace(); ++it){
+    if ((*it)->geomType() == GEntity::Plane)continue;
+    //    printf("FACE %d with %d triangles\n",(*it)->tag(),(*it)->triangles.size());
+    for (unsigned int i=0;i<(*it)->triangles.size(); i++){
+      //Obj += MFaceGFaceDistance ( (*it)->triangles[i] , *it );
+      Obj = std::max(Obj,MFaceGFaceDistance ( (*it)->triangles[i] , *it ));
+    }
+  }
+  
+  printf("DISTANCE TO GEOMETRY : 1D AND 2D PART %22.15E\n",Obj);
+  fprintf(f,"};\n");
+  fclose(f);
+  return Obj;
+}
+
+
 bool OptHOM::addBndObjGrad(double factor, double &Obj, alglib::real_1d_array &gradObj)
+{
+  // set the mesh to its present position
+  std::vector<SPoint3> xyz,uvw;
+  mesh.getGEntityPositions(xyz,uvw);
+  mesh.updateGEntityPositions();
+
+  //could be better (e.g. store the model in the Mesh:: datastrucure)
+
+  GModel *gm = GModel::current();
+
+  // for all model edges, compute the error between the geometry and the mesh
+
+  maxDistCAD = 0.0;
+  double distCAD = 0.0;
+
+  for (GModel::eiter it = gm->firstEdge(); it != gm->lastEdge(); ++it){
+    // do not do straight lines
+    if ((*it)->geomType() == GEntity::Line)continue;
+    // look at all mesh lines
+
+    std::vector<bool> doWeCompute((*it)->lines.size());
+    for (unsigned int i=0;i<(*it)->lines.size(); i++){
+      doWeCompute[i] = false;
+      for (unsigned int j=0;j<(*it)->lines[i]->getNumVertices(); j++){
+	int index = mesh.getFreeVertexStartIndex((*it)->lines[i]->getVertex(j));
+	if (index >=0){
+	  doWeCompute[i] = true;
+	  continue;
+	}
+      }
+    }
+
+    std::vector<double> dist((*it)->lines.size());
+    for (unsigned int i=0;i<(*it)->lines.size(); i++){
+      if (doWeCompute[i]){
+	// compute the distance from the geometry to the mesh
+	dist[i] = MLineGEdgeDistance ( (*it)->lines[i] , *it );
+	maxDistCAD = std::max(maxDistCAD,dist[i]);
+	distCAD += dist [i] * factor;
+      }
+    }
+    // be clever to compute the derivative : iterate on all
+    // Distance = \sum_{lines} Distance (line, GEdge)
+    // For a high order vertex, we compute the derivative only by 
+    // recomputing the distance to one only line     
+    const double eps = 1.e-6;
+    for (unsigned int i=0;i<(*it)->lines.size(); i++){
+      if (doWeCompute[i]){
+	for (int j=2 ; j<(*it)->lines[i]->getNumVertices()  ; j++){
+	  MVertex *v = (*it)->lines[i]->getVertex(j);
+	  int index = mesh.getFreeVertexStartIndex(v);
+	  //	printf("%d %d (%d %d)\n",v->getNum(),index,v->onWhat()->tag(),v->onWhat()->dim());
+	  if (index >= 0){
+	    double t;
+	    v->getParameter(0,t);
+	    SPoint3 pp (v->x(),v->y(),v->z());
+	    GPoint gp = (*it)->point(t+eps);
+	    v->setParameter(0,t+eps);
+	    v->setXYZ(gp.x(),gp.y(),gp.z());
+	    double dist2 = MLineGEdgeDistance ( (*it)->lines[i] , *it );
+	    double deriv = (dist2 - dist[i])/eps;	
+	    v->setXYZ(pp.x(),pp.y(),pp.z());
+	    v->setParameter(0,t);
+	    //	  printf("%g %g %g\n",dist[i],dist2, MLineGEdgeDistance ( (*it)->lines[i] , *it ));
+	    // get the index of the vertex
+	    gradObj[index] += deriv * factor;	
+	  }
+	}
+      }
+      //    printf("done\n");
+      // For a low order vertex classified on the GEdge, we recompute 
+    // two distances for the two MLines connected to the vertex
+      for (unsigned int i=0;i<(*it)->lines.size()-1; i++){
+	MVertex *v =  (*it)->lines[i]->getVertex(1);
+	int index = mesh.getFreeVertexStartIndex(v);
+	if (index >= 0){
+	  double t;
+	  v->getParameter(0,t);
+	  SPoint3 pp (v->x(),v->y(),v->z());
+	  GPoint gp = (*it)->point(t+eps);
+	  v->setParameter(0,t+eps);
+	  v->setXYZ(gp.x(),gp.y(),gp.z());
+	  MLine *l1 = (*it)->lines[i];
+	  MLine *l2 = (*it)->lines[i+1];
+	  //	printf("%d %d -- %d %d\n",l1->getVertex(0)->getNum(),l1->getVertex(1)->getNum(),l2->getVertex(0)->getNum(),l2->getVertex(1)->getNum());
+	  double deriv = 
+	    (MLineGEdgeDistance ( l1 , *it ) - dist[i])  /eps +
+	    (MLineGEdgeDistance ( l2 , *it ) - dist[i+1])/eps;      
+	  v->setXYZ(pp.x(),pp.y(),pp.z());
+	  v->setParameter(0,t);
+	  gradObj[index] += deriv * factor;
+	}
+      }
+    } 
+  }
+  //  printf("computing distance : 1D part %12.5E\n",distCAD);
+
+  // now the 3D part !
+
+  std::vector<std::vector<SVector3> > gsfT;
+  computeGradSFAtNodes ( (*gm->firstFace())->triangles[0],gsfT);
+
+  std::map<MVertex*,SVector3> normalsToCAD;
+
+
+  for(GModel::fiter it = gm->firstFace(); it != gm->lastFace(); ++it){
+    // do not do plane surfaces
+    if ((*it)->geomType() == GEntity::Plane)continue;
+    std::map<MTriangle*,double> dist;
+
+    std::vector<bool> doWeCompute((*it)->triangles.size());
+    for (unsigned int i=0;i<(*it)->triangles.size(); i++){
+      doWeCompute[i] = false;
+      for (unsigned int j=0;j<(*it)->triangles[i]->getNumVertices(); j++){
+	int index = mesh.getFreeVertexStartIndex((*it)->triangles[i]->getVertex(j));
+	if (index >=0){
+	  doWeCompute[i] = true;	  
+	}
+      }
+      if (doWeCompute[i]){
+	for (unsigned int j=0;j<(*it)->triangles[i]->getNumVertices(); j++){
+	  MVertex *v = (*it)->triangles[i]->getVertex(j);
+	  if (normalsToCAD.find(v) == normalsToCAD.end()){
+	    SPoint2 p_cad;
+	    reparamMeshVertexOnFace(v, *it, p_cad);
+	    SVector3 tg_cad = (*it)->normal(p_cad);
+	    tg_cad.normalize(); 
+	    normalsToCAD[v] = tg_cad;
+	  }
+	}
+      }
+    }
+
+    for (unsigned int i=0;i<(*it)->triangles.size(); i++){
+      // compute the distance from the geometry to the mesh
+      if(doWeCompute[i]){
+	const double d = MFaceGFaceDistance ( (*it)->triangles[i] , *it , &gsfT, &normalsToCAD);
+	dist[(*it)->triangles[i]] = d;
+	maxDistCAD = std::max(maxDistCAD,d);
+	distCAD += d * factor;
+      }
+    }
+
+    // be clever again to compute the derivatives 
+    const double eps = 1.e-6;
+    for (unsigned int i=0;i<(*it)->triangles.size(); i++){
+      if(doWeCompute[i]){
+	for (unsigned int j=0;j<(*it)->triangles[i]->getNumVertices(); j++){
+	  //    for (; itm !=v2t.end(); ++itm){
+	  MVertex   *v = (*it)->triangles[i]->getVertex(j);
+	  if(v->onWhat()->dim() == 1){
+	    int index = mesh.getFreeVertexStartIndex(v);
+	    if (index >= 0){
+	      MTriangle *t = (*it)->triangles[i];
+	      GEdge *ge = v->onWhat()->cast2Edge();
+	      double t_;
+	      v->getParameter(0,t_);
+	      SPoint3 pp (v->x(),v->y(),v->z());
+	      GPoint gp = ge->point(t_+eps);
+	      v->setParameter(0,t_+eps);
+	      v->setXYZ(gp.x(),gp.y(),gp.z());
+	      const double distT = dist[t];
+	      double deriv =  (MFaceGFaceDistance ( t , *it , &gsfT, &normalsToCAD) - distT)  /eps;
+	      v->setXYZ(pp.x(),pp.y(),pp.z());
+	      v->setParameter(0,t_);
+	      gradObj[index] += deriv * factor;
+	    }
+	  }
+	  
+	  if(v->onWhat() == *it){
+	    int index = mesh.getFreeVertexStartIndex(v);
+	    if (index >= 0){
+	      MTriangle *t = (*it)->triangles[i];
+	      double uu,vv;
+	      v->getParameter(0,uu);
+	      v->getParameter(1,vv);
+	      SPoint3 pp (v->x(),v->y(),v->z());
+	      
+	      const double distT = dist[t];
+	      
+	      GPoint gp = (*it)->point(uu+eps,vv);
+	      v->setParameter(0,uu+eps);
+	      v->setXYZ(gp.x(),gp.y(),gp.z());
+	      double deriv = (MFaceGFaceDistance ( t , *it, &gsfT, &normalsToCAD ) - distT)  /eps;
+	      v->setXYZ(pp.x(),pp.y(),pp.z());
+	      v->setParameter(0,uu);
+	      gradObj[index] += deriv * factor;
+	      
+	      gp = (*it)->point(uu,vv+eps);
+	      v->setParameter(1,vv+eps);
+	      v->setXYZ(gp.x(),gp.y(),gp.z());	  
+	      deriv = (MFaceGFaceDistance ( t , *it, &gsfT, &normalsToCAD ) - distT)  /eps;
+	      v->setXYZ(pp.x(),pp.y(),pp.z());
+	      v->setParameter(1,vv);
+	      gradObj[index+1] += deriv * factor;	
+	    }
+	  }
+	}
+      }
+    }
+  }
+  mesh.updateGEntityPositions(xyz,uvw);
+  Obj +=distCAD;
+  //  printf("computing distance : 2D part %12.5E\n",distCAD);
+  //  printf("%22.15E\n",distCAD);
+}
+
+bool OptHOM::addBndObjGrad2(double factor, double &Obj, alglib::real_1d_array &gradObj)
+{
+  maxDistCAD = 0.0;
+
+  std::vector<double> gradF;
+  double DISTANCE = 0.0;
+  for (int iEl = 0; iEl < mesh.nEl(); iEl++) {
+    double f;
+    if (mesh.bndDistAndGradients(iEl, f, gradF, geomTol)) {
+      maxDistCAD = std::max(maxDistCAD,f);
+      DISTANCE += f;
+      Obj += f * factor;
+      for (size_t i = 0; i < mesh.nPCEl(iEl); ++i){
+        gradObj[mesh.indPCEl(iEl, i)] += gradF[i] * factor;
+	//	printf("gradf[%d] = %12.5E\n",i,gradF[i]*factor);
+      }
+    }
+  }
+  //  printf("DIST = %12.5E\n",DISTANCE);
+  return true;
+
+}
+
+
+bool OptHOM::addBndObjGrad(double factor, double &Obj, std::vector<double> &gradObj)
 {
   maxDistCAD = 0.0;
 
@@ -129,6 +585,8 @@ bool OptHOM::addBndObjGrad(double factor, double &Obj, alglib::real_1d_array &gr
 
 }
 
+
+
 bool OptHOM::addMetricMinObjGrad(double &Obj, alglib::real_1d_array &gradObj)
 {
 
@@ -177,24 +635,92 @@ bool OptHOM::addDistObjGrad(double Fact, double Fact2, double &Obj,
   }
   if (nbBnd != 0) avgDist /= nbBnd;
 
+  //  printf("DISTANCE %22.15E\n",avgDist);
+
   return true;
 }
 
+bool OptHOM::addDistObjGrad(double Fact, double Fact2, double &Obj,
+                            std::vector<double> &gradObj)
+{
+  maxDist = 0;
+  avgDist = 0;
+  int nbBnd = 0;
+
+  for (int iFV = 0; iFV < mesh.nFV(); iFV++) {
+    const double Factor = invLengthScaleSq*(mesh.forced(iFV) ? Fact : Fact2);
+    const double dSq = mesh.distSq(iFV), dist = sqrt(dSq);
+    Obj += Factor * dSq;
+    std::vector<double> gDSq(mesh.nPCFV(iFV));
+    mesh.gradDistSq(iFV,gDSq);
+    for (int iPC = 0; iPC < mesh.nPCFV(iFV); iPC++)
+      gradObj[mesh.indPCFV(iFV,iPC)] += Factor*gDSq[iPC];
+    maxDist = std::max(maxDist, dist);
+    avgDist += dist;
+    nbBnd++;
+  }
+  if (nbBnd != 0) avgDist /= nbBnd;
+
+  return true;
+}
+
+
+// FIXME TEST
+// Assume a unit square centered on 0,0
+// fct is 
+//class toto : public simpleFunction<double> 
+//{
+//public :
+//  double operator () (double x, double y, double z) const{
+//    const double r = sqrt(x*x+y*y);
+//    const double r0 = .3;
+//  const double f = atan(20*(x));
+//    return f;
+//  }
+//};
+
+// Gmsh's (cheaper) version of the optimizer 
+void OptHOM::evalObjGrad(std::vector<double> &x, 
+			 double &Obj,
+			 bool gradsNeeded, 
+                         std::vector<double> &gradObj)
+{
+  mesh.updateMesh(x.data());
+  Obj = 0.;
+  for (unsigned int i = 0; i < gradObj.size(); i++) gradObj[i] = 0.;
+
+  /// control Jacobians
+  addJacObjGrad(Obj, gradObj);
+  /// Control distance to the straight sided mesh
+  addDistObjGrad(lambda, lambda2, Obj, gradObj);
+  if(_optimizeCAD)
+    addBndObjGrad(lambda3, Obj, gradObj);
+  
+}
+
+
 void OptHOM::evalObjGrad(const alglib::real_1d_array &x, double &Obj,
                          alglib::real_1d_array &gradObj)
 {
+  NEVAL++;
   mesh.updateMesh(x.getcontent());
 
   Obj = 0.;
   for (int i = 0; i < gradObj.length(); i++) gradObj[i] = 0.;
 
+  //  printf("Computing Obj : ");
+  /// control Jacobians
   addJacObjGrad(Obj, gradObj);
+  /// Control distance to the straight sided mesh
   addDistObjGrad(lambda, lambda2, Obj, gradObj);
+
   if(_optimizeMetricMin)
     addMetricMinObjGrad(Obj, gradObj);
   if(_optimizeCAD)
     addBndObjGrad(lambda3, Obj, gradObj);
-  //  printf("maxDistCAD = %12.5E distMax = %12.5E Obj %12.5E\n",maxDistCAD,distance_max,Obj);
+
+  //  printf("Obj = %12.5E\n",Obj);
+
   if ((minJac > barrier_min) && (maxJac < barrier_max || !_optimizeBarrierMax) && (maxDistCAD < distance_max|| !_optimizeCAD) ) {
     Msg::Info("Reached %s (%g %g) requirements, setting null gradient",
               _optimizeMetricMin ? "svd" : "jacobian", minJac, maxJac);
@@ -209,6 +735,13 @@ void evalObjGradFunc(const alglib::real_1d_array &x, double &Obj,
   (static_cast<OptHOM*>(HOInst))->evalObjGrad(x, Obj, gradObj);
 }
 
+void evalObjGradFunc(std::vector<double> &x, double &Obj, bool needGrad, 
+                     std::vector<double> &gradObj, void *HOInst)
+{
+  (static_cast<OptHOM*>(HOInst))->evalObjGrad(x, Obj, true, gradObj);
+}
+
+
 void OptHOM::recalcJacDist()
 {
   maxDist = 0;
@@ -269,30 +802,17 @@ void OptHOM::calcScale(alglib::real_1d_array &scale)
     for (int iPC = 0; iPC < mesh.nPCFV(iFV); iPC++)
       scale[mesh.indPCFV(iFV,iPC)] = scaleFV[iPC];
   }
-
-//  std::vector<double> inSize(mesh.nEl());
-//  mesh.elInSize(inSize);
-//  for (int iEl = 0; iEl < mesh.nEl(); iEl++) {
-////    std::cout << "DBGTT: inSize[" << iEl << "] = " << inSize[iEl] << std::endl;
-//    for (int iPCEl = 0; iPCEl < mesh.nPCEl(iEl); iPCEl++)
-//      scale[mesh.indPCEl(iEl,iPCEl)] *= inSize[iEl];
-//  }
-
-//  for (int iEl = 0; iEl < mesh.nEl(); iEl++) {
-//    std::vector<double> sJ(mesh.nBezEl(iEl)), gSJ(mesh.nBezEl(iEl)*mesh.nPCEl(iEl));
-//    mesh.scaledJacAndGradients(iEl,sJ,gSJ);
-//    for (int iPC = 0; iPC < mesh.nPCEl(iEl); iPC++) {
-//      double grad = 0.;
-//      for (int l = 0; l < mesh.nBezEl(iEl); l++) grad = std::max(grad,fabs(gSJ[mesh.indGSJ(iEl,l,iPC)]));
-//      scale[mesh.indPCEl(iEl,iPC)] *= mesh.nBezEl(iEl)/grad;
-////      std::cout << "DBGTT: scale[" << mesh.indPCEl(iEl,iPC) << "] = " << scale[mesh.indPCEl(iEl,iPC)] << std::endl;
-//    }
-//  }
-
 }
 
-void OptHOM::OptimPass(alglib::real_1d_array &x,
-                       const alglib::real_1d_array &initGradObj, int itMax)
+void OptHOM::OptimPass(std::vector<double> &x, int itMax)
+{
+  Msg::Info("--- In-house Optimization pass with initial jac. range (%g, %g), jacBar = %g",
+            minJac, maxJac, jacBar);
+  GradientDescent (evalObjGradFunc, x, this);
+} 
+ 
+
+void OptHOM::OptimPass(alglib::real_1d_array &x, int itMax)
 {
 
   static const double EPSG = 0.;
@@ -359,7 +879,8 @@ void OptHOM::OptimPass(alglib::real_1d_array &x,
   }
 }
 
-int OptHOM::optimize(double weightFixed, double weightFree, double weightCAD, double b_min,
+
+int OptHOM::optimize_inhouse(double weightFixed, double weightFree, double weightCAD, double b_min,
                      double b_max, bool optimizeMetricMin, int pInt,
                      int itMax, int optPassMax, int optCAD, double distanceMax, double tolerance)
 {
@@ -415,9 +936,109 @@ int OptHOM::optimize(double weightFixed, double weightFree, double weightCAD, do
 
   int ITER = 0;
   bool minJacOK = true;
+
+  while (minJac < barrier_min || (maxDistCAD > distance_max && _optimizeCAD)) {
+    const double startMinJac = minJac;
+    NEVAL = 0;
+    OptimPass(x, itMax);
+    printf("######  NEVAL = %d\n",NEVAL);
+    recalcJacDist();
+    jacBar = (minJac > 0.) ? 0.9*minJac : 1.1*minJac;
+    if (_optimizeCAD)   jacBar = std::min(jacBar,barrier_min); 
+    if (ITER ++ > optPassMax) {
+      minJacOK = (minJac > barrier_min && (maxDistCAD < distance_max || !_optimizeCAD));
+      break;
+    }
+    if (fabs((minJac-startMinJac)/startMinJac) < 0.01) {
+      Msg::Info("Stagnation in minJac detected, stopping optimization");
+      minJacOK = false;
+      break;
+    }
+  }
+
+  ITER = 0;
+  if (minJacOK && (!_optimizeMetricMin)) {
+    _optimizeBarrierMax = true;
+    jacBar =  1.1 * maxJac;
+    while (maxJac > barrier_max ) {
+      const double startMaxJac = maxJac;
+      OptimPass(x, itMax);
+      recalcJacDist();
+      jacBar =  1.1 * maxJac;
+      if (ITER ++ > optPassMax) break;
+      if (fabs((maxJac-startMaxJac)/startMaxJac) < 0.01) {
+        Msg::Info("Stagnation in maxJac detected, stopping optimization");
+        break;
+      }
+    }
+  }
+
+  Msg::Info("Optimization done Range (%g,%g)", minJac, maxJac);
+
+  if (minJac > barrier_min && maxJac < barrier_max) return 1;
+  if (minJac > 0.0) return 0;
+  return -1;
+}
+
+
+int OptHOM::optimize(double weightFixed, double weightFree, double weightCAD, double b_min,
+			     double b_max, bool optimizeMetricMin, int pInt,
+			     int itMax, int optPassMax, int optCAD, double distanceMax, double tolerance)
+{
+  barrier_min = b_min;
+  barrier_max = b_max;
+  distance_max = distanceMax;
+  progressInterv = pInt;
+//  powM = 4;
+//  powP = 3;
+
+  _optimizeMetricMin = optimizeMetricMin;
+  _optimizeCAD = optCAD;
+  // Set weights & length scale for non-dimensionalization
+  lambda = weightFixed;
+  lambda2 = weightFree;
+  lambda3 = weightCAD;
+  geomTol = tolerance;
+  std::vector<double> dSq(mesh.nEl());
+  mesh.distSqToStraight(dSq);
+  const double maxDSq = *max_element(dSq.begin(),dSq.end());
+  if (maxDSq < 1.e-10) {                                        // Length scale for non-dim. distance
+    std::vector<double> sSq(mesh.nEl());
+    mesh.elSizeSq(sSq);
+    const double maxSSq = *max_element(sSq.begin(),sSq.end());
+    invLengthScaleSq = 1./maxSSq;
+  }
+  else invLengthScaleSq = 1./maxDSq;
+
+  // Set initial guess
+  std::vector<double> x(mesh.nPC());
+  mesh.getUvw(x.data());
+
+  // Calculate initial performance
+  recalcJacDist();
+  initMaxDist = maxDist;
+  initAvgDist = avgDist;
+
+  const double jacBarStart = (minJac > 0.) ? 0.9*minJac : 1.1*minJac;
+  jacBar = jacBarStart;
+
+  _optimizeBarrierMax = false;
+  // Calculate initial objective function value and gradient
+  initObj = 0.;
+  std::vector<double>gradObj(mesh.nPC());
+  for (int i = 0; i < mesh.nPC(); i++) gradObj[i] = 0.;
+  evalObjGrad(x, initObj, true, gradObj);
+
+  Msg::Info("Start optimizing %d elements (%d vertices, %d free vertices, %d variables) "
+            "with min barrier %g and max. barrier %g", mesh.nEl(), mesh.nVert(),
+            mesh.nFV(), mesh.nPC(), barrier_min, barrier_max);
+
+  int ITER = 0;
+  bool minJacOK = true;
+
   while (minJac < barrier_min || (maxDistCAD > distance_max && _optimizeCAD)) {
     const double startMinJac = minJac;
-    OptimPass(x, gradObj, itMax);
+    OptimPass(x, itMax);
     recalcJacDist();
     jacBar = (minJac > 0.) ? 0.9*minJac : 1.1*minJac;
     if (_optimizeCAD)   jacBar = std::min(jacBar,barrier_min); 
@@ -438,7 +1059,7 @@ int OptHOM::optimize(double weightFixed, double weightFree, double weightCAD, do
     jacBar =  1.1 * maxJac;
     while (maxJac > barrier_max ) {
       const double startMaxJac = maxJac;
-      OptimPass(x, gradObj, itMax);
+      OptimPass(x, itMax);
       recalcJacDist();
       jacBar =  1.1 * maxJac;
       if (ITER ++ > optPassMax) break;
diff --git a/contrib/HighOrderMeshOptimizer/OptHOM.h b/contrib/HighOrderMeshOptimizer/OptHOM.h
index 7ca9db1572..3eaf09d89c 100644
--- a/contrib/HighOrderMeshOptimizer/OptHOM.h
+++ b/contrib/HighOrderMeshOptimizer/OptHOM.h
@@ -34,6 +34,7 @@
 #include <math.h>
 #include "GmshConfig.h"
 #include "OptHomMesh.h"
+#include "simpleFunction.h"
 
 #if defined(HAVE_BFGS)
 
@@ -52,15 +53,20 @@ public:
   // mesh is invalid : some jacobians cannot be made positive
   int optimize(double lambda, double lambda2, double lambda3, double barrier_min, double barrier_max,
                bool optimizeMetricMin, int pInt, int itMax, int optPassMax, int optimizeCAD, double optCADDistMax, double tolerance);
+  int optimize_inhouse(double weightFixed, double weightFree, double weightCAD, double b_min,
+		       double b_max, bool optimizeMetricMin, int pInt,
+		       int itMax, int optPassMax, int optCAD, double distanceMax, double tolerance);
   void recalcJacDist();
   inline void getJacDist(double &minJ, double &maxJ, double &maxD, double &avgD);
   void updateMesh(const alglib::real_1d_array &x);
+  void evalObjGrad(std::vector<double> &x, double &Obj, bool gradsNeeded, 
+		   std::vector<double> &gradObj);
+
   void evalObjGrad(const alglib::real_1d_array &x, double &Obj,
                    alglib::real_1d_array &gradObj);
   void printProgress(const alglib::real_1d_array &x, double Obj);
 
   double barrier_min, barrier_max, distance_max, geomTol;
-
  private:
   double lambda, lambda2, lambda3, jacBar, invLengthScaleSq;
   int iter, progressInterv; // Current iteration, interval of iterations for reporting
@@ -71,14 +77,20 @@ public:
                             // true : fixed barrier min + moving barrier max
   bool _optimizeCAD; // false : do not minimize the distance between mesh and CAD
                      // true : minimize the distance between mesh and CAD
+  bool addApproximationErrorObjGrad(double Fact, double &Obj, alglib::real_1d_array &gradObj, simpleFunction<double>& fct);
   bool addJacObjGrad(double &Obj, alglib::real_1d_array &gradObj);
-  bool addBndObjGrad(double Fact, double &Obj, alglib::real_1d_array &gradObj);
+  bool addJacObjGrad(double &Obj, std::vector<double> &);
+  bool addBndObjGrad (double Fact, double &Obj, alglib::real_1d_array &gradObj);
+  bool addBndObjGrad2(double Fact, double &Obj, alglib::real_1d_array &gradObj);
+  bool addBndObjGrad(double Fact, double &Obj, std::vector<double> &gradObj);
   bool addMetricMinObjGrad(double &Obj, alglib::real_1d_array &gradObj);
+  bool addDistObjGrad(double Fact, double Fact2, double &Obj,
+                      std::vector<double> &gradObj);
   bool addDistObjGrad(double Fact, double Fact2, double &Obj,
                       alglib::real_1d_array &gradObj);
   void calcScale(alglib::real_1d_array &scale);
-  void OptimPass(alglib::real_1d_array &x, const alglib::real_1d_array &initGradObj,
-                 int itMax);
+  void OptimPass(alglib::real_1d_array &x, int itMax);
+  void OptimPass(std::vector<double> &x, int itMax);
 };
 
 void OptHOM::getJacDist(double &minJ, double &maxJ, double &maxD, double &avgD)
@@ -86,6 +98,10 @@ void OptHOM::getJacDist(double &minJ, double &maxJ, double &maxD, double &avgD)
   minJ = minJac; maxJ = maxJac; maxD = maxDist; avgD = avgDist;
 }
 
+double distanceToGeometry(GModel *gm);
+void distanceFromElementsToGeometry(GModel *gm, int dim, std::map<MElement*,double> &distances);
+
+
 #endif
 
 #endif
diff --git a/contrib/HighOrderMeshOptimizer/OptHomIntegralBoundaryDist.cpp b/contrib/HighOrderMeshOptimizer/OptHomIntegralBoundaryDist.cpp
index a19f49210c..3f3f27f19f 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomIntegralBoundaryDist.cpp
+++ b/contrib/HighOrderMeshOptimizer/OptHomIntegralBoundaryDist.cpp
@@ -9,11 +9,13 @@
 
 parametricLineNodalBasis::parametricLineNodalBasis(const nodalBasis &basis,
                                                    const std::vector<SPoint3> &xyz):
-  _basis(basis), _xyz(xyz) {};
+  _basis(basis), _xyz(xyz) , psi(_xyz.size())
+{
+}
+
 
 SPoint3 parametricLineNodalBasis::operator()(double xi) const
 {
-  std::vector<double> psi(_xyz.size());
   SPoint3 p(0, 0, 0);
   _basis.f(-1 + 2 * xi, 0, 0, &psi[0]);
   for (size_t j = 0; j < psi.size(); ++j) {
@@ -245,7 +247,7 @@ double computeBndDistG_(GEdge *edge, std::vector<double> & p, // the model edge
 		       const nodalBasis &basis, const std::vector<SPoint3> &xyz,
 		       const unsigned int NN) // the mesh edge
 {
-  const unsigned int N = 256;
+  const unsigned int N = 200;
   std::vector<int> o;
   o.push_back(0);
   for (unsigned int i=2; i < p.size();i++)o.push_back(i);
@@ -298,15 +300,15 @@ double computeBndDistG(GEdge *edge, std::vector<double> & p, // the model edge
 		       const nodalBasis &basis, const std::vector<SPoint3> &xyz,
 		       double tolerance) // the mesh edge
 {
-  int N = 4;
+  int N = 5;
   double d = computeBndDistG_(edge, p, basis, xyz, N);
 
-  //    printf("GO !!\n");
+  //  printf("GO !!\n");
   while (1){
     N *= 2;
     double dp = computeBndDistG_(edge, p, basis, xyz, N);
-    //        printf("%12.5E %12.5E %12.5E %12.5E\n",d,dp,fabs(d - dp),tolerance);
-    if (fabs(d - dp) < tolerance) // Richardson with assumed linear convergence ...
+    //    printf("N %d %12.5E %12.5E %12.5E %12.5E\n",N,d,dp,fabs(d - dp),tolerance);
+    if (fabs(d - dp) < tolerance*(d+dp)) // Richardson with assumed linear convergence ...
       return dp;
     d = dp;
   }
@@ -362,7 +364,7 @@ double computeDeviationOfTangents(GEdge *edge,
   //  parametricLineGEdge l1 = parametricLineGEdge(edge,p[0],p[p.size()-1]);
   parametricLineNodalBasis l2 = parametricLineNodalBasis(basis, xyz);
   double  deviation = 0;
-  double ddeviation = 0;
+  //  double ddeviation = 0;
   std::vector<int> o;
   o.push_back(0);
   for (unsigned int i=2; i < p.size();i++)o.push_back(i);
@@ -373,27 +375,27 @@ double computeDeviationOfTangents(GEdge *edge,
   for (unsigned int i=0; i<p.size();i++){
     const double u = basis.points(o[i],0);
     SVector3 xp = edge->firstDer (p[o[i]]);
-    SVector3 xpp = edge->secondDer (p[o[i]]);
-    const double nxp = xp.norm();
-    const double onxp = 1./nxp;
-    SVector3 c = (onxp*onxp*onxp)*(xpp*nxp-xp*dot(xp,xpp)*onxp);
+    //    SVector3 xpp = edge->secondDer (p[o[i]]);
+    //    const double nxp = xp.norm();
+    //    const double onxp = 1./nxp;
+    //    SVector3 c = (onxp*onxp*onxp)*(xpp*nxp-xp*dot(xp,xpp)*onxp);
 
     SVector3 t_mesh_edge  = l2.derivative(0.5*(1+u));
-    SVector3 c2  = l2.curvature(0.5*(1+u));
+    //    SVector3 c2  = l2.curvature(0.5*(1+u));
     //    GPoint p0 = edge->point(p[o[i]]);
     //    SPoint3 p1 = l2 (0.5*(1+u));
     //    printf("%g = %g %g vs %g %g\n",u,p0.x(),p0.y(),p1.x(),p1.y());
     xp.normalize();
     t_mesh_edge.normalize();
     SVector3 diff1 = (dot(xp, t_mesh_edge) > 0) ? xp -  t_mesh_edge : xp +  t_mesh_edge;
-    SVector3 diff2 = (dot(c, c2) > 0) ? c -  c2 : c +  c2;
+    //    SVector3 diff2 = (dot(c, c2) > 0) ? c -  c2 : c +  c2;
     //printf("%g %g %g vs %g %g %g diff %g %g %g\n",c.x(),c.y(),c.z(),c2.x(),c2.y(),c2.z(),diff2.x(),diff2.y(),diff2.z());
     //    printf("%g %g %g vs %g %g %g val %g\n",t_model_edge.x(),t_model_edge.y(),t_model_edge.z(),
     //	   t_mesh_edge.x(),t_mesh_edge.y(),t_mesh_edge.z(),c.norm());
     //     deviation = std::max(diff1.norm(),deviation);
     //    ddeviation = std::max(diff2.norm(),ddeviation);
      deviation += diff1.norm();
-    ddeviation += diff2.norm();
+     //    ddeviation += diff2.norm();
   }
   const double h =  dx.norm();
   //  printf ("%g %g\n",deviation * h,ddeviation * h * h * 0.5);
diff --git a/contrib/HighOrderMeshOptimizer/OptHomIntegralBoundaryDist.h b/contrib/HighOrderMeshOptimizer/OptHomIntegralBoundaryDist.h
index 3a9e5db2ce..a904663e54 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomIntegralBoundaryDist.h
+++ b/contrib/HighOrderMeshOptimizer/OptHomIntegralBoundaryDist.h
@@ -34,6 +34,7 @@ class parametricLineNodalBasis : public parametricLine
 {
   const nodalBasis &_basis;
   const std::vector<SPoint3> &_xyz;
+  mutable std::vector<double> psi;
  public :
   parametricLineNodalBasis(const nodalBasis &basis, const std::vector<SPoint3> &xyz);
   virtual SPoint3 operator()(double xi) const;
diff --git a/contrib/HighOrderMeshOptimizer/OptHomMesh.cpp b/contrib/HighOrderMeshOptimizer/OptHomMesh.cpp
index 5c7614c6d2..21fcc23c4d 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomMesh.cpp
+++ b/contrib/HighOrderMeshOptimizer/OptHomMesh.cpp
@@ -153,6 +153,17 @@ void Mesh::calcScaledNormalEl2D(const std::map<MElement*,GEntity*> &element2enti
 
 }
 
+int Mesh::getFreeVertexStartIndex(MVertex* vert)
+{
+  std::map<MVertex*,int>::iterator itVert = _startPC4FV.find(vert);
+  if (itVert == _startPC4FV.end()) {
+    //    Msg::Fatal("OptHOM Error : cannot find free vertex %d class %d %d (%d free vertices)",vert->getNum(),vert->onWhat()->tag(),vert->onWhat()->dim(),_freeVert.size());
+    return -1;
+  }  
+  return itVert->second;    
+}
+
+
 int Mesh::addVert(MVertex* vert)
 {
   std::vector<MVertex*>::iterator itVert = find(_vert.begin(),_vert.end(),vert);
@@ -172,6 +183,7 @@ int Mesh::addFreeVert(MVertex* vert, const int iV, const int nPCV,
   if (itVert == _freeVert.end()) {
     const int iStart = (_startPCFV.size() == 0)? 0 : _startPCFV.back()+_nPCFV.back();
     const bool forcedV = (vert->onWhat()->dim() < 2) || (toFix.find(vert) != toFix.end());
+    _startPC4FV[vert] = iStart;
     _freeVert.push_back(vert);
     _paramFV.push_back(param);
     _fv2V.push_back(iV);
@@ -230,12 +242,41 @@ void Mesh::elInSize(std::vector<double> &s)
     s[iEl] = fabs(_el[iEl]->getInnerRadius());
 }
 
-void Mesh::updateGEntityPositions()
+void Mesh::getGEntityPositions(std::vector<SPoint3> &xyz,
+			       std::vector<SPoint3> &uvw) 
 {
+  xyz.resize(nVert());
+  uvw.resize(nFV());
   for (int iV = 0; iV < nVert(); iV++)
-    _vert[iV]->setXYZ(_xyz[iV].x(),_xyz[iV].y(),_xyz[iV].z());
+    xyz[iV] = SPoint3(_vert[iV]->x(),_vert[iV]->y(),_vert[iV]->z());
+  for (int iFV = 0; iFV < nFV(); iFV++){
+    MVertex *v = _freeVert[iFV];
+    if (v->onWhat()->dim() == 1){
+      double t;
+      v->getParameter(0,t);
+      uvw[iFV] = SPoint3(t,0,0);
+    }
+    if (v->onWhat()->dim() == 2){
+      double uu,vv;
+      v->getParameter(0,uu);
+      v->getParameter(1,vv);
+      uvw[iFV] = SPoint3(uu,vv,0);
+    }
+  }
+}
+
+void Mesh::updateGEntityPositions(const std::vector<SPoint3> &xyz,
+				  const std::vector<SPoint3> &uvw)
+{
+  for (int iV = 0; iV < nVert(); iV++)
+    _vert[iV]->setXYZ(xyz[iV].x(),xyz[iV].y(),xyz[iV].z());
   for (int iFV = 0; iFV < nFV(); iFV++)
-    _paramFV[iFV]->exportParamCoord(_uvw[iFV]);
+      _paramFV[iFV]->exportParamCoord(uvw[iFV]);
+}
+
+void Mesh::updateGEntityPositions()
+{
+  updateGEntityPositions(_xyz,_uvw);
 }
 
 void Mesh::metricMinAndGradients(int iEl, std::vector<double> &lambda,
@@ -294,6 +335,70 @@ void Mesh::metricMinAndGradients(int iEl, std::vector<double> &lambda,
   }
 }
 
+void Mesh::approximationErrorAndGradients(int iEl, double &f, std::vector<double> &gradF, double eps, 
+					  simpleFunction<double> &fct)
+{
+  std::vector<SPoint3> _xyz_temp;
+  for (int iV = 0; iV < nVert(); iV++){
+    _xyz_temp.push_back(SPoint3( _vert[iV]->x(), _vert[iV]->y(), _vert[iV]->z()));
+    _vert[iV]->setXYZ(_xyz[iV].x(),_xyz[iV].y(),_xyz[iV].z());
+  }
+
+  MElement *element = _el[iEl];
+
+  f = approximationError (fct, element);
+  // FIME
+  //  if (iEl < 1)printf("approx error elem %d = %g\n",iEl,f);
+  int currentId = 0;
+  // compute the size of the gradient 
+  // depends on how many dofs exist per vertex (0,1,2 or 3)
+  for (size_t i = 0; i < element->getNumVertices(); ++i) {
+    if (_el2FV[iEl][i] >= 0) {// some free coordinates
+      currentId += _nPCFV[_el2FV[iEl][i]];
+    }
+  }
+  gradF.clear();
+  gradF.resize(currentId, 0.);
+  currentId = 0;
+  for (size_t i = 0; i < element->getNumVertices(); ++i) {
+    if (_el2FV[iEl][i] >= 0) {// some free coordinates
+      MVertex *v =  element->getVertex(i);
+      // vertex classified on a model edge
+      if (_nPCFV[_el2FV[iEl][i]] == 1){
+	double t = _uvw[_el2FV[iEl][i]].x();
+	GEdge *ge = (GEdge*)v->onWhat();
+	SPoint3 p (v->x(),v->y(),v->z()); 
+	GPoint d = ge->point(t+eps);
+	v->setXYZ(d.x(),d.y(),d.z());
+	double f_d = approximationError (fct, element);
+	gradF[currentId++] = (f_d-f)/eps;
+	if (iEl < 1)printf("df = %g\n",(f_d-f)/eps);
+	v->setXYZ(p.x(),p.y(),p.z());
+      }
+      else if (_nPCFV[_el2FV[iEl][i]] == 2){
+	double uu = _uvw[_el2FV[iEl][i]].x();
+	double vv = _uvw[_el2FV[iEl][i]].y();
+	GFace *gf = (GFace*)v->onWhat();
+	SPoint3 p (v->x(),v->y(),v->z()); 
+	GPoint  d = gf->point(uu+eps,vv);
+	v->setXYZ(d.x(),d.y(),d.z());
+	double f_u = approximationError (fct, element);
+	gradF[currentId++] = (f_u-f)/eps;
+	d = gf->point(uu,vv+eps);
+	v->setXYZ(d.x(),d.y(),d.z());
+	double f_v = approximationError (fct, element);
+	gradF[currentId++] = (f_v-f)/eps;
+	v->setXYZ(p.x(),p.y(),p.z());
+	//	if (iEl < 1)printf("df = %g %g\n",(f_u-f)/eps,(f_v-f)/eps);
+      }
+    }
+  }
+  for (int iV = 0; iV < nVert(); iV++)
+    _vert[iV]->setXYZ(_xyz_temp[iV].x(),_xyz_temp[iV].y(),_xyz_temp[iV].z());
+  
+}
+
+
 bool Mesh::bndDistAndGradients(int iEl, double &f , std::vector<double> &gradF, double eps)
 {
   MElement *element = _el[iEl];
@@ -303,9 +408,11 @@ bool Mesh::bndDistAndGradients(int iEl, double &f , std::vector<double> &gradF,
     return false;
 
   int currentId = 0;
+  bool touches_boundary = false;
   std::vector<int> vertex2param(element->getNumVertices());
   for (size_t i = 0; i < element->getNumVertices(); ++i) {
     if (_el2FV[iEl][i] >= 0) {
+      if ( _nPCFV[_el2FV[iEl][i]] == 1) touches_boundary = true;
       vertex2param[i] = currentId;
       currentId += _nPCFV[_el2FV[iEl][i]];
     }
@@ -315,6 +422,7 @@ bool Mesh::bndDistAndGradients(int iEl, double &f , std::vector<double> &gradF,
   gradF.clear();
   gradF.resize(currentId, 0.);
 
+  if (!touches_boundary){/*printf("ele %d\n",iEl);*/return true;}
   const nodalBasis &elbasis = *element->getFunctionSpace();
   bool edgeFound = false;
   for (int iEdge = 0; iEdge < element->getNumEdges(); ++iEdge) {
diff --git a/contrib/HighOrderMeshOptimizer/OptHomMesh.h b/contrib/HighOrderMeshOptimizer/OptHomMesh.h
index f1f48c9fed..696a19b0c2 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomMesh.h
+++ b/contrib/HighOrderMeshOptimizer/OptHomMesh.h
@@ -37,6 +37,8 @@
 #include "MVertex.h"
 #include "ParamCoord.h"
 #include "polynomialBasis.h"
+#include "simpleFunction.h"
+#include "approximationError.h"
 
 class Mesh
 {
@@ -54,7 +56,9 @@ public:
   inline int nPCEl(int iEl) { return _indPCEl[iEl].size(); }
   inline const int &indPCEl(int iEl, int iPC) { return _indPCEl[iEl][iPC]; }
   inline const int &nBezEl(int iEl) { return _nBezEl[iEl]; }
+  int getFreeVertexStartIndex(MVertex* vert);
 
+  void approximationErrorAndGradients(int iEl, double &f, std::vector<double> &gradF, double eps, simpleFunction<double> &fct);
   void metricMinAndGradients(int iEl, std::vector<double> &sJ, std::vector<double> &gSJ);
   void scaledJacAndGradients(int iEl, std::vector<double> &sJ, std::vector<double> &gSJ);
   bool bndDistAndGradients(int iEl, double &f , std::vector<double> &gradF, double eps);
@@ -73,6 +77,11 @@ public:
   void elInSize(std::vector<double> &s);
 
   void updateGEntityPositions();
+  void updateGEntityPositions(const std::vector<SPoint3> &xyz,
+			      const std::vector<SPoint3> &uvw);
+
+  void getGEntityPositions(std::vector<SPoint3> &xyz,
+			   std::vector<SPoint3> &uvw) ;
   void writeMSH(const char *filename);
 
 private:
@@ -81,6 +90,8 @@ private:
   int _nPC;
   // Use fast Jacobian estimation?
   bool _fastJacEval;
+  // Free vertex id numbers
+  std::map<MVertex*,int> _startPC4FV;
   // List of elements
   std::vector<MElement*> _el;
   // Normals to 2D elements for Jacobian regularization and scaling
@@ -130,8 +141,14 @@ private:
   {
     return indJB3DBase(_nNodEl[iEl],l,i,j,m);
   }
+public: 
+  double approximationErr(int iEl, simpleFunction<double> &f)
+  {
+    return approximationError (f, _el[iEl]);
+  }
 };
 
+
 double Mesh::distSq(int iFV)
 {
   SPoint3 d = _xyz[_fv2V[iFV]]-_ixyz[_fv2V[iFV]];
diff --git a/contrib/HighOrderMeshOptimizer/OptHomRun.cpp b/contrib/HighOrderMeshOptimizer/OptHomRun.cpp
index d3449d5538..0df9010057 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomRun.cpp
+++ b/contrib/HighOrderMeshOptimizer/OptHomRun.cpp
@@ -230,6 +230,8 @@ static bool testTriSphereIntersect(SPoint3 A, SPoint3 B, SPoint3 C,
 }
 
 // Approximate test of intersection element with circle/sphere by sampling
+// This function takes 99% of the CPU !!
+
 static bool testElInDist(const SPoint3 p, double limDist, MElement *el)
 {
   const double limDistSq = limDist*limDist;
@@ -451,11 +453,11 @@ static void optimizeConnectedBlobs(const vertElVecMap &vertex2elements,
     if (temp.mesh.nPC() == 0)
       Msg::Info("Blob %i has no degree of freedom, skipping", i+1);
     else
-      success = temp.optimize(p.weightFixed, p.weightFree, p.optCADWeight, p.BARRIER_MIN,
+      success = temp.optimize_inhouse(p.weightFixed, p.weightFree, p.optCADWeight, p.BARRIER_MIN,
                               p.BARRIER_MAX, false, samples, p.itMax, p.optPassMax, p.optCAD, p.optCADDistMax, p.discrTolerance);
     if (success >= 0 && p.BARRIER_MIN_METRIC > 0) {
       Msg::Info("Jacobian optimization succeed, starting svd optimization");
-      success = temp.optimize(p.weightFixed, p.weightFree, p.optCADWeight, p.BARRIER_MIN_METRIC, p.BARRIER_MAX,
+      success = temp.optimize_inhouse(p.weightFixed, p.weightFree, p.optCADWeight, p.BARRIER_MIN_METRIC, p.BARRIER_MAX,
                               true, samples, p.itMax, p.optPassMax, p.optCAD, p.optCADDistMax,p.discrTolerance);
     }
     double minJac, maxJac, distMaxBND, distAvgBND;
@@ -537,11 +539,11 @@ static void optimizeOneByOne
       std::ostringstream ossI1;
       ossI1 << "initial_blob-" << iBadEl << ".msh";
       opt->mesh.writeMSH(ossI1.str().c_str());
-      success = opt->optimize(p.weightFixed, p.weightFree, p.optCADWeight, p.BARRIER_MIN,
+      success = opt->optimize_inhouse(p.weightFixed, p.weightFree, p.optCADWeight, p.BARRIER_MIN,
                               p.BARRIER_MAX, false, samples, p.itMax, p.optPassMax, p.optCAD, p.optCADDistMax,p.discrTolerance);
       if (success >= 0 && p.BARRIER_MIN_METRIC > 0) {
         Msg::Info("Jacobian optimization succeed, starting svd optimization");
-        success = opt->optimize(p.weightFixed, p.weightFree, p.optCADWeight, p.BARRIER_MIN_METRIC,
+        success = opt->optimize_inhouse(p.weightFixed, p.weightFree, p.optCADWeight, p.BARRIER_MIN_METRIC,
                                 p.BARRIER_MAX, true, samples, p.itMax, p.optPassMax, p.optCAD, p.optCADDistMax,p.discrTolerance);
       }
 
@@ -581,6 +583,13 @@ static void optimizeOneByOne
 #endif
 
 #include "OptHomIntegralBoundaryDist.h"
+
+double ComputeDistanceToGeometry (GModel* gm)
+{
+  return distanceToGeometry(gm);
+}
+
+
 double ComputeDistanceToGeometry (GEntity *ge , int distanceDefinition, double tolerance)
 {
   double maxd = 0.0;
@@ -598,7 +607,8 @@ double ComputeDistanceToGeometry (GEntity *ge , int distanceDefinition, double t
       }
     }
   }
-  if (distanceDefinition == 2 && NUM) return sum / (double)NUM;
+  if (distanceDefinition == 2) return sum;
+  if (distanceDefinition == 6) return sum;
   return maxd;
 }
 
@@ -619,6 +629,10 @@ void HighOrderMeshOptimizer(GModel *gm, OptHomParameters &p)
   std::map<MElement*,GEntity*> element2entity;
   elSet badasses;
   double maxdist = 0.;                                                  // TODO: To be cleaned?
+
+  std::map<MElement*,double> distances;
+  distanceFromElementsToGeometry(gm, p.dim,distances);
+
   for (int iEnt = 0; iEnt < entities.size(); ++iEnt) {
     GEntity* &entity = entities[iEnt];
     if (entity->dim() != p.dim || (p.onlyVisible && !entity->getVisibility())) continue;
@@ -630,8 +644,12 @@ void HighOrderMeshOptimizer(GModel *gm, OptHomParameters &p)
       double jmin, jmax;
       MElement *el = entity->getMeshElement(iEl);
       if (el->getDim() == p.dim) {
+	// FIXME TEST
+	//        badasses.insert(el);
         if (p.optCAD) {
-          const double DISTE =computeBndDist(el,2,fabs(p.discrTolerance));
+	  //          const double DISTE =computeBndDist(el,2,fabs(p.discrTolerance));
+	  const double DISTE =distances[el];
+	  //	  if (DISTE > 0)printf("El %d dist %12.5E vs %12.5E\n",iEl,DISTE,p.optCADDistMax);
           maxdist = std::max(DISTE, maxdist);
           if (DISTE > p.optCADDistMax) badasses.insert(el);
         }
diff --git a/contrib/HighOrderMeshOptimizer/OptHomRun.h b/contrib/HighOrderMeshOptimizer/OptHomRun.h
index cceed2f94c..2f2e1d9f81 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomRun.h
+++ b/contrib/HighOrderMeshOptimizer/OptHomRun.h
@@ -78,5 +78,6 @@ void HighOrderMeshOptimizer(GModel *gm, OptHomParameters &p);
 void HighOrderMeshOptimizerNew(GModel *gm, OptHomParameters &p);
 // distanceDefinition 1) Hausdorff 2) Area/Length 3) Frechet (not done)
 double ComputeDistanceToGeometry (GEntity *ge , int distanceDefinition,double tolerance) ;
+double ComputeDistanceToGeometry (GModel*gm);
 
 #endif
-- 
GitLab