diff --git a/Mesh/HighOrder.cpp b/Mesh/HighOrder.cpp
index d6cf536c544b81c538a6e094656c9e02f53d3148..626300d2cdb69ccecdeb7673b704a8ac9edf4a89 100644
--- a/Mesh/HighOrder.cpp
+++ b/Mesh/HighOrder.cpp
@@ -22,6 +22,9 @@
 #include "fullMatrix.h"
 #include "BasisFactory.h"
 
+
+// --------- Functions that help optimizing placement of points on geometry -----------
+
 // The aim here is to build a polynomial representation that consist
 // in polynomial segments of equal length
 
@@ -119,269 +122,193 @@ static bool computeEquidistantParameters(GFace *gf, double u0, double uN,
   return true;
 }
 
-static void getEdgeVertices(GEdge *ge, MElement *ele, std::vector<MVertex*> &ve,
-                            edgeContainer &edgeVertices, bool linear,
-                            int nPts = 1)
+
+// --------- Creation of high-order edge vertices -----------
+
+static bool getEdgeVerticesonGeo(GEdge *ge, const MEdge &edge, std::vector<MVertex*> &ve, int nPts = 1)
 {
-  if(ge->geomType() == GEntity::DiscreteCurve ||
-     ge->geomType() == GEntity::BoundaryLayerCurve)
-    linear = true;
+    MVertex *v0 = edge.getVertex(0), *v1 = edge.getVertex(1);
+    double u0 = 0., u1 = 0., US[100];
+    bool reparamOK = reparamMeshVertexOnEdge(v0, ge, u0);
+    if(ge->periodic(0) && ge->getEndVertex()->getNumMeshVertices() > 0 &&
+       v1 == ge->getEndVertex()->mesh_vertices[0])
+      u1 = ge->parBounds(0).high();
+    else
+      reparamOK &= reparamMeshVertexOnEdge(v1, ge, u1);
 
-  for(int i = 0; i < ele->getNumEdges(); i++){
-    MEdge edge = ele->getEdge(i);
-    std::pair<MVertex*, MVertex*> p(edge.getMinVertex(), edge.getMaxVertex());
-    if(edgeVertices.count(p)){
-      if(edge.getVertex(0) == edge.getMinVertex())
-        ve.insert(ve.end(), edgeVertices[p].begin(), edgeVertices[p].end());
-      else
-        ve.insert(ve.end(), edgeVertices[p].rbegin(), edgeVertices[p].rend());
+    if(reparamOK){
+      double relax = 1.;
+      while (1){
+        if(computeEquidistantParameters(ge, std::min(u0,u1), std::max(u0,u1),
+                                        nPts + 2, US, relax))
+          break;
+
+        relax /= 2.0;
+        if(relax < 1.e-2) break;
+      }
+      if(relax < 1.e-2){
+        Msg::Warning
+          ("Failed to compute equidistant parameters (relax = %g, value = %g) "
+           "for edge %d-%d parametrized with %g %g on GEdge %d",
+           relax, US[1], v0->getNum(), v1->getNum(),u0,u1,ge->tag());
+        reparamOK = false;
+      }
     }
-    else{
+    else
+      Msg::Error("Cannot reparam a mesh Vertex in high order meshing");
+    if (!reparamOK) return false;
+
+    for(int j = 0; j < nPts; j++) {
+      MVertex *v;
+      int count = u0<u1? j + 1 : nPts + 1  - (j + 1);
+      GPoint pc = ge->point(US[count]);
+      v = new MEdgeVertex(pc.x(), pc.y(), pc.z(), ge,US[count]);
+      // this destroys the ordering of the mesh vertices on the edge
+      ve.push_back(v);
+    }
+
+    return true;
+}
+
+static bool getEdgeVerticesonGeo(GFace *gf, const MEdge &edge, std::vector<MVertex*> &ve, int nPts = 1)
+{
       MVertex *v0 = edge.getVertex(0), *v1 = edge.getVertex(1);
-      std::vector<MVertex*> temp;
-      double u0 = 0., u1 = 0., US[100];
-      bool reparamOK = true;
-      if(!linear) {
-        reparamOK &= reparamMeshVertexOnEdge(v0, ge, u0);
-        if(ge->periodic(0) && ge->getEndVertex()->getNumMeshVertices() > 0 &&
-           v1 == ge->getEndVertex()->mesh_vertices[0])
-          u1 = ge->parBounds(0).high();
-        else
-          reparamOK &= reparamMeshVertexOnEdge(v1, ge, u1);
-        if(reparamOK){
-          double relax = 1.;
-          while (1){
-            if(computeEquidistantParameters(ge, std::min(u0,u1), std::max(u0,u1),
-                                            nPts + 2, US, relax))
-              break;
-
-            relax /= 2.0;
-            if(relax < 1.e-2)
-              break;
-          }
-          if(relax < 1.e-2){
-            Msg::Warning
-              ("Failed to compute equidistant parameters (relax = %g, value = %g) "
-               "for edge %d-%d parametrized with %g %g on GEdge %d linear %d",
-               relax, US[1], v0->getNum(), v1->getNum(),u0,u1,ge->tag(), linear);
-            reparamOK = false;
+      SPoint2 p0, p1;
+      double US[100], VS[100];
+      bool reparamOK = reparamMeshEdgeOnFace(v0, v1, gf, p0, p1);
+      if(reparamOK) {
+        if (nPts >= 30)
+          computeEquidistantParameters(gf, p0[0], p1[0], p0[1], p1[1], nPts + 2, US, VS);
+        else {
+          US[0]      =  p0[0];
+          VS[0]      =  p0[1];
+          US[nPts+1] =  p1[0];
+          VS[nPts+1] =  p1[1];
+          for(int j = 0; j < nPts; j++){
+            const double t = (double)(j + 1) / (nPts + 1);
+            SPoint3 pc = edge.interpolate(t);
+            SPoint2 guess = p0 * (1.-t) + p1 * t;
+            GPoint gp = gf->closestPoint(pc, guess);
+            if(gp.succeeded()){
+              US[j+1] = gp.u();
+              VS[j+1] = gp.v();
+            }
+            else{
+              US[j+1] = guess.x();
+              VS[j+1] = guess.y();
+            }
           }
         }
-        else{
-          Msg::Error("Cannot reparam a mesh Vertex in high order meshing");
-        }
       }
+      else
+        Msg::Error("Cannot reparam a mesh Vertex in high order meshing");
+      if (!reparamOK) return false;
+
       for(int j = 0; j < nPts; j++){
-        const double t = (double)(j + 1)/(nPts + 1);
-        double uc = (1. - t) * u0 + t * u1; // can be wrong, that's ok
-        MVertex *v;
-        if(linear || !reparamOK || uc < std::min(u0,u1) || uc > std::max(u0,u1)){
-          if (!linear)
-            Msg::Warning("We don't have a valid parameter on curve %d-%d",
-                         v0->getNum(), v1->getNum());
-          // we don't have a (valid) parameter on the curve
-          SPoint3 pc = edge.interpolate(t);
-          v = new MVertex(pc.x(), pc.y(), pc.z(), ge);
-        }
-        else {
-          int count = u0<u1? j + 1 : nPts + 1  - (j + 1);
-          GPoint pc = ge->point(US[count]);
-          v = new MEdgeVertex(pc.x(), pc.y(), pc.z(), ge,US[count]);
-        }
-        temp.push_back(v);
-        // this destroys the ordering of the mesh vertices on the edge
-        ge->mesh_vertices.push_back(v);
+        GPoint pc = gf->point(US[j + 1], VS[j + 1]);
+        MVertex *v = new MFaceVertex(pc.x(), pc.y(), pc.z(), gf, US[j + 1], VS[j + 1]);
         ve.push_back(v);
       }
 
-      if(edge.getVertex(0) == edge.getMinVertex())
-        edgeVertices[p].insert(edgeVertices[p].end(), temp.begin(), temp.end());
-      else
-        edgeVertices[p].insert(edgeVertices[p].end(), temp.rbegin(), temp.rend());
-    }
+      return true;
+}
+
+static void interpVerticesInExistingEdge(GEntity *ge, const MEdge &edge,
+                                         std::vector<MVertex*> &veEdge, int nPts)
+{
+  for(int j = 0; j < nPts; j++) {
+    const double t = (double)(j + 1)/(nPts + 1);
+    SPoint3 pc = edge.interpolate(t);
+    MVertex *v = new MVertex(pc.x(), pc.y(), pc.z(), ge);
+    veEdge.push_back(v);
   }
 }
 
+// Get new interior vertices for a 1D element
+static void getEdgeVertices(GEdge *ge, MElement *ele, std::vector<MVertex*> &ve,
+                            std::vector<MVertex*> &newHOVert, edgeContainer &edgeVertices,
+                            bool linear, int nPts = 1)
+{
+  if(ge->geomType() == GEntity::DiscreteCurve ||
+     ge->geomType() == GEntity::BoundaryLayerCurve)
+    linear = true;
+
+  MEdge edge = ele->getEdge(0);
+  std::pair<MVertex*, MVertex*> p(edge.getMinVertex(), edge.getMaxVertex());
+  std::vector<MVertex*> veEdge;
+  bool gotVertOnGeo = linear ? false : getEdgeVerticesonGeo(ge, edge, veEdge, nPts);    // Get vertices on geometry if asked
+  if (!gotVertOnGeo)                                                                    // If not on geometry, create from mesh interpolation
+    interpVerticesInExistingEdge(ge, edge, veEdge, nPts);
+  newHOVert.insert(newHOVert.end(), veEdge.begin(), veEdge.end());
+  if(edge.getVertex(0) == edge.getMinVertex())                                          // Add newly created vertices to list
+    edgeVertices[p].insert(edgeVertices[p].end(), veEdge.begin(), veEdge.end());
+  else
+    edgeVertices[p].insert(edgeVertices[p].end(), veEdge.rbegin(), veEdge.rend());
+  ve.insert(ve.end(), veEdge.begin(), veEdge.end());
+}
+
+// Get new interior vertices for an edge in a 2D element
 static void getEdgeVertices(GFace *gf, MElement *ele, std::vector<MVertex*> &ve,
-                            edgeContainer &edgeVertices, bool linear,
-                            int nPts = 1)
+                            std::vector<MVertex*> &newHOVert, edgeContainer &edgeVertices,
+                            bool linear, int nPts = 1)
 {
   if(gf->geomType() == GEntity::DiscreteSurface ||
      gf->geomType() == GEntity::BoundaryLayerSurface)
     linear = true;
 
-  for(int i = 0; i < ele->getNumEdges(); i++){
+  for(int i = 0; i < ele->getNumEdges(); i++) {
     MEdge edge = ele->getEdge(i);
     std::pair<MVertex*, MVertex*> p(edge.getMinVertex(), edge.getMaxVertex());
-    if(edgeVertices.count(p)){
+    std::vector<MVertex*> veEdge;
+    if(edgeVertices.count(p)) {                                                             // Vertices already exist
       if(edge.getVertex(0) == edge.getMinVertex())
-        ve.insert(ve.end(), edgeVertices[p].begin(), edgeVertices[p].end());
+        veEdge.assign(edgeVertices[p].begin(), edgeVertices[p].end());
       else
-        ve.insert(ve.end(), edgeVertices[p].rbegin(), edgeVertices[p].rend());
+        veEdge.assign(edgeVertices[p].rbegin(), edgeVertices[p].rend());
     }
-    else{
-      MVertex *v0 = edge.getVertex(0), *v1 = edge.getVertex(1);
-      SPoint2 p0, p1;
-      double US[100], VS[100];
-      bool reparamOK = true;
-      if(!linear){
-        reparamOK = reparamMeshEdgeOnFace(v0, v1, gf, p0, p1);
-        if(reparamOK) {
-	  if (nPts >= 30)computeEquidistantParameters(gf, p0[0], p1[0], p0[1], p1[1], nPts + 2,
-						     US, VS);
-	  else {
-	    US[0]      =  p0[0];
-	    VS[0]      =  p0[1];
-	    US[nPts+1] =  p1[0];
-	    VS[nPts+1] =  p1[1];
-	    for(int j = 0; j < nPts; j++){
-	      const double t = (double)(j + 1) / (nPts + 1);
-	      SPoint3 pc = edge.interpolate(t);
-	      SPoint2 guess = p0 * (1.-t) + p1 * t;
-	      GPoint gp = gf->closestPoint(pc, guess);
-	      if(gp.succeeded()){
-		US[j+1] = gp.u();
-		VS[j+1] = gp.v();
-	      }
-	      else{
-		US[j+1] = guess.x();
-		VS[j+1] = guess.y();
-	      }
-	    }
-	  }
-	}
-      }
-      std::vector<MVertex*> temp;
-      for(int j = 0; j < nPts; j++){
-        const double t = (double)(j + 1) / (nPts + 1);
-        MVertex *v;
-        if(linear || !reparamOK){
-          // we don't have (valid) parameters on the surface
-          SPoint3 pc = edge.interpolate(t);
-          v = new MVertex(pc.x(), pc.y(), pc.z(), gf);
-        }
-        else{
-          GPoint pc = gf->point(US[j + 1], VS[j + 1]);
-          v = new MFaceVertex(pc.x(), pc.y(), pc.z(), gf, US[j + 1], VS[j + 1]);
-        }
-        temp.push_back(v);
-        gf->mesh_vertices.push_back(v);
-        ve.push_back(v);
-      }
-      if(edge.getVertex(0) == edge.getMinVertex())
-        edgeVertices[p].insert(edgeVertices[p].end(), temp.begin(), temp.end());
+    else {                                                                                  // Vertices do not exist, create them
+      bool gotVertOnGeo = linear ? false : getEdgeVerticesonGeo(gf, edge, veEdge, nPts);    // Get vertices on geometry if asked
+      if (!gotVertOnGeo)                                                                    // If not on geometry, create from mesh interpolation
+        interpVerticesInExistingEdge(gf, edge, veEdge, nPts);
+      newHOVert.insert(newHOVert.end(), veEdge.begin(), veEdge.end());
+      if(edge.getVertex(0) == edge.getMinVertex())                                          // Add newly created vertices to list
+        edgeVertices[p].insert(edgeVertices[p].end(), veEdge.begin(), veEdge.end());
       else
-        edgeVertices[p].insert(edgeVertices[p].end(), temp.rbegin(), temp.rend());
+        edgeVertices[p].insert(edgeVertices[p].end(), veEdge.rbegin(), veEdge.rend());
     }
+    ve.insert(ve.end(), veEdge.begin(), veEdge.end());
   }
 }
 
+// Get new interior vertices for an edge in a 3D element
 static void getEdgeVertices(GRegion *gr, MElement *ele, std::vector<MVertex*> &ve,
-                            edgeContainer &edgeVertices, bool linear,
-                            int nPts = 1)
+                            std::vector<MVertex*> &newHOVert, edgeContainer &edgeVertices,
+                            bool linear, int nPts = 1)
 {
-  for(int i = 0; i < ele->getNumEdges(); i++){
+  for(int i = 0; i < ele->getNumEdges(); i++) {
     MEdge edge = ele->getEdge(i);
     std::pair<MVertex*, MVertex*> p(edge.getMinVertex(), edge.getMaxVertex());
-    if(edgeVertices.count(p)){
+    std::vector<MVertex*> veEdge;
+    if(edgeVertices.count(p)) {                                                             // Vertices already exist
       if(edge.getVertex(0) == edge.getMinVertex())
-        ve.insert(ve.end(), edgeVertices[p].begin(), edgeVertices[p].end());
+        veEdge.assign(edgeVertices[p].begin(), edgeVertices[p].end());
       else
-        ve.insert(ve.end(), edgeVertices[p].rbegin(), edgeVertices[p].rend());
+        veEdge.assign(edgeVertices[p].rbegin(), edgeVertices[p].rend());
     }
-    else{
-      std::vector<MVertex*> temp;
-      for(int j = 0; j < nPts; j++){
-        double t = (double)(j + 1) / (nPts + 1);
-        SPoint3 pc = edge.interpolate(t);
-        MVertex *v = new MVertex(pc.x(), pc.y(), pc.z(), gr);
-        temp.push_back(v);
-        gr->mesh_vertices.push_back(v);
-        ve.push_back(v);
-      }
-      if(edge.getVertex(0) == edge.getMinVertex())
-        edgeVertices[p].insert(edgeVertices[p].end(), temp.begin(), temp.end());
+    else {                                                                                  // Vertices do not exist, create them
+      interpVerticesInExistingEdge(gr, edge, veEdge, nPts);
+      newHOVert.insert(newHOVert.end(), veEdge.begin(), veEdge.end());
+      if(edge.getVertex(0) == edge.getMinVertex())                                          // Add newly created vertices to list
+        edgeVertices[p].insert(edgeVertices[p].end(), veEdge.begin(), veEdge.end());
       else
-        edgeVertices[p].insert(edgeVertices[p].end(), temp.rbegin(), temp.rend());
+        edgeVertices[p].insert(edgeVertices[p].end(), veEdge.rbegin(), veEdge.rend());
     }
+    ve.insert(ve.end(), veEdge.begin(), veEdge.end());
   }
 }
 
-static void getFaceVertices(GFace *gf, MElement *incomplete, MElement *ele,
-                            std::vector<MVertex*> &vf, faceContainer &faceVertices,
-                            bool linear, int nPts = 1)
-{
-  if(gf->geomType() == GEntity::DiscreteSurface ||
-     gf->geomType() == GEntity::BoundaryLayerSurface)
-    linear = true;
 
-  for(int i = 0; i < ele->getNumFaces(); i++){
-    MFace face = ele->getFace(i);
-    faceContainer::iterator fIter = faceVertices.find(face);
-    if(fIter != faceVertices.end()){
-      vf.insert(vf.end(), fIter->second.begin(), fIter->second.end());
-    }
-    else{
-      std::vector<MVertex*> &vtcs = faceVertices[face];
-      SPoint2 pts[1000];
-      bool reparamOK = true;
-      if(!linear){
-        for(int k = 0; k < incomplete->getNumVertices(); k++)
-          reparamOK &= reparamMeshVertexOnFace(incomplete->getVertex(k), gf, pts[k]);
-      }
-      int start = face.getNumVertices() * (nPts + 1);
-      const fullMatrix<double> &points = ele->getFunctionSpace(nPts + 1)->points;
-      for(int k = start; k < points.size1(); k++){
-        MVertex *v;
-        const double t1 = points(k, 0);
-        const double t2 = points(k, 1);
-        if(linear){
-          SPoint3 pc = face.interpolate(t1, t2);
-          v = new MVertex(pc.x(), pc.y(), pc.z(), gf);
-        }
-        else{
-          double X(0), Y(0), Z(0), GUESS[2] = {0, 0};
-          double sf[1256];
-          incomplete->getShapeFunctions(t1, t2, 0, sf);
-          for (int j = 0; j < incomplete->getNumShapeFunctions(); j++){
-            MVertex *vt = incomplete->getShapeFunctionNode(j);
-            X += sf[j] * vt->x();
-            Y += sf[j] * vt->y();
-            Z += sf[j] * vt->z();
-            if (reparamOK){
-              GUESS[0] += sf[j] * pts[j][0];
-              GUESS[1] += sf[j] * pts[j][1];
-            }
-          }
-          if(reparamOK){
-            GPoint gp = gf->point(SPoint2(GUESS[0], GUESS[1]));
-            // closest point is not necessary (slow and for high quality HO
-            // meshes it should be optimized anyway afterwards + closest point
-            // is still buggy (e.g. BFGS for a plane Ruled Surface)
-            // GPoint gp = gf->closestPoint(SPoint3(X, Y, Z), GUESS);
-            if (gp.g()){
-              v = new MFaceVertex(gp.x(), gp.y(), gp.z(), gf, gp.u(), gp.v());
-            }
-            else{
-              v = new MVertex(X, Y, Z, gf);
-            }
-          }
-          else{
-            GPoint gp = gf->closestPoint(SPoint3(X, Y, Z), GUESS);
-            if(gp.succeeded())
-              v = new MVertex(gp.x(), gp.y(), gp.z(), gf);
-            else
-              v = new MVertex(X, Y, Z, gf);
-          }
-        }
-        // should be expensive -> induces a new search each time
-        vtcs.push_back(v);
-        gf->mesh_vertices.push_back(v);
-        vf.push_back(v);
-      }
-    }
-  }
-}
+// --------- Creation of high-order face vertices -----------
 
 static void reorientTrianglePoints(std::vector<MVertex*> &vtcs, int orientation,
                                    bool swap)
@@ -486,7 +413,7 @@ static void reorientQuadPoints(std::vector<MVertex*> &vtcs, int orientation,
   }
 }
 
-static int getNewFacePoints(int numPrimaryFacePoints, int nPts, fullMatrix<double> &points)
+static int getNewFacePointsInFace(int numPrimaryFacePoints, int nPts, fullMatrix<double> &points)
 {
   int start = 0;
 
@@ -523,99 +450,310 @@ static int getNewFacePoints(int numPrimaryFacePoints, int nPts, fullMatrix<doubl
   return start;
 }
 
-static void getFaceVertices(GRegion *gr, MElement *ele, std::vector<MVertex*> &vf,
+static int getNewFacePointsInVolume(MElement *incomplete, int nPts, fullMatrix<double> &points)
+{
+
+  int startFace = 0;
+
+  switch (incomplete->getType()){
+  case TYPE_TET :
+    switch (nPts){
+    case 0:
+    case 1: return -1;
+    case 2: points = BasisFactory::getNodalBasis(MSH_TET_20)->points; break;
+    case 3: points = BasisFactory::getNodalBasis(MSH_TET_35)->points; break;
+    case 4: points = BasisFactory::getNodalBasis(MSH_TET_56)->points; break;
+    case 5: points = BasisFactory::getNodalBasis(MSH_TET_84)->points; break;
+    case 6: points = BasisFactory::getNodalBasis(MSH_TET_120)->points; break;
+    case 7: points = BasisFactory::getNodalBasis(MSH_TET_165)->points; break;
+    case 8: points = BasisFactory::getNodalBasis(MSH_TET_220)->points; break;
+    case 9: points = BasisFactory::getNodalBasis(MSH_TET_286)->points; break;
+    default:
+      Msg::Error("getFaceAndInteriorVertices not implemented for order %i", nPts+1);
+      break;
+    }
+    startFace = 4 + nPts*6;
+    break;
+  case TYPE_HEX :
+    switch (nPts){
+    case 0: return -1;
+    case 1: points = BasisFactory::getNodalBasis(MSH_HEX_27)->points; break;
+    case 2: points = BasisFactory::getNodalBasis(MSH_HEX_64)->points; break;
+    case 3: points = BasisFactory::getNodalBasis(MSH_HEX_125)->points; break;
+    case 4: points = BasisFactory::getNodalBasis(MSH_HEX_216)->points; break;
+    case 5: points = BasisFactory::getNodalBasis(MSH_HEX_343)->points; break;
+    case 6: points = BasisFactory::getNodalBasis(MSH_HEX_512)->points; break;
+    case 7: points = BasisFactory::getNodalBasis(MSH_HEX_729)->points; break;
+    case 8: points = BasisFactory::getNodalBasis(MSH_HEX_1000)->points; break;
+    default :
+      Msg::Error("getFaceAndInteriorVertices not implemented for order %i", nPts+1);
+      break;
+    }
+    startFace = 8 + nPts*12 ;
+    break;
+  case TYPE_PRI :
+    switch (nPts){
+    case 0: return -1;
+    case 1: points = BasisFactory::getNodalBasis(MSH_PRI_18)->points; break;
+    case 2: points = BasisFactory::getNodalBasis(MSH_PRI_40)->points; break;
+    case 3: points = BasisFactory::getNodalBasis(MSH_PRI_75)->points; break;
+    case 4: points = BasisFactory::getNodalBasis(MSH_PRI_126)->points; break;
+    case 5: points = BasisFactory::getNodalBasis(MSH_PRI_196)->points; break;
+    case 6: points = BasisFactory::getNodalBasis(MSH_PRI_288)->points; break;
+    case 7: points = BasisFactory::getNodalBasis(MSH_PRI_405)->points; break;
+    case 8: points = BasisFactory::getNodalBasis(MSH_PRI_550)->points; break;
+    default:
+      Msg::Error("getFaceAndInteriorVertices not implemented for order %i", nPts+1);
+      break;
+    }
+    startFace = 6 + nPts*9;
+    break;
+  case TYPE_PYR:
+    switch (nPts){
+    case 0:
+    case 1: return -1;
+    case 2: points = BasisFactory::getNodalBasis(MSH_PYR_30)->points; break;
+    case 3: points = BasisFactory::getNodalBasis(MSH_PYR_55)->points; break;
+    case 4: points = BasisFactory::getNodalBasis(MSH_PYR_91)->points; break;
+    case 5: points = BasisFactory::getNodalBasis(MSH_PYR_140)->points; break;
+    case 6: points = BasisFactory::getNodalBasis(MSH_PYR_204)->points; break;
+    case 7: points = BasisFactory::getNodalBasis(MSH_PYR_285)->points; break;
+    case 8: points = BasisFactory::getNodalBasis(MSH_PYR_385)->points; break;
+    default :
+      Msg::Error("getFaceAndInteriorVertices not implemented for order %i", nPts+1);
+      break;
+    }
+    startFace = 5 + nPts*8;
+    break;
+
+  }
+
+  return startFace;
+}
+
+static void getFaceVerticesOnGeo(GFace *gf, MElement *incomplete, const MFace &face,
+                            std::vector<MVertex*> &vf, int nPts = 1)
+{
+  SPoint2 pts[1000];
+  bool reparamOK = true;
+  for(int k = 0; k < incomplete->getNumVertices(); k++)
+    reparamOK &= reparamMeshVertexOnFace(incomplete->getVertex(k), gf, pts[k]);
+  fullMatrix<double> points;
+  int start = getNewFacePointsInFace(face.getNumVertices(), nPts, points);
+  for(int k = start; k < points.size1(); k++) {
+    MVertex *v;
+    const double t1 = points(k, 0);
+    const double t2 = points(k, 1);
+    double X(0), Y(0), Z(0), GUESS[2] = {0, 0};
+    double sf[1256];
+    incomplete->getShapeFunctions(t1, t2, 0, sf);
+    for (int j = 0; j < incomplete->getNumShapeFunctions(); j++){
+      MVertex *vt = incomplete->getShapeFunctionNode(j);
+      X += sf[j] * vt->x();
+      Y += sf[j] * vt->y();
+      Z += sf[j] * vt->z();
+      if (reparamOK){
+        GUESS[0] += sf[j] * pts[j][0];
+        GUESS[1] += sf[j] * pts[j][1];
+      }
+    }
+    if(reparamOK){
+      GPoint gp = gf->point(SPoint2(GUESS[0], GUESS[1]));
+      // closest point is not necessary (slow and for high quality HO
+      // meshes it should be optimized anyway afterwards + closest point
+      // is still buggy (e.g. BFGS for a plane Ruled Surface)
+      // GPoint gp = gf->closestPoint(SPoint3(X, Y, Z), GUESS);
+      if (gp.g()){
+        v = new MFaceVertex(gp.x(), gp.y(), gp.z(), gf, gp.u(), gp.v());
+      }
+      else{
+        v = new MVertex(X, Y, Z, gf);
+      }
+    }
+    else{
+      GPoint gp = gf->closestPoint(SPoint3(X, Y, Z), GUESS);
+      if(gp.succeeded())
+        v = new MVertex(gp.x(), gp.y(), gp.z(), gf);
+      else
+        v = new MVertex(X, Y, Z, gf);
+    }
+    vf.push_back(v);
+  }
+}
+
+static void interpVerticesInExistingFace(GEntity *ge, const MFace &face,
+                                         std::vector<MVertex*> &veFace, int nPts)
+{
+  fullMatrix<double> points;
+  int start = getNewFacePointsInFace(face.getNumVertices(), nPts, points);
+  for(int k = start; k < points.size1(); k++){
+    const double t1 = points(k, 0);
+    const double t2 = points(k, 1);
+    SPoint3 pc = face.interpolate(t1, t2);
+    MVertex *v = new MVertex(pc.x(), pc.y(), pc.z(), ge);
+    veFace.push_back(v);
+  }
+//  MElement *interpEl = getStraightInterpEl(face);
+//  for (int k = start; k < points.size1(); k++) {
+//    double t1 = points(k, 0);
+//    double t2 = points(k, 1);
+//    SPoint3 pos;
+//    interpEl->pnt(t1, t2, 0, pos);
+//    MVertex* v = new MVertex(pos.x(), pos.y(), pos.z(), gf);
+//    vf.push_back(v);
+//  }
+//  delete interpEl;
+}
+
+
+//static void interpVerticesInIncompleteFace(GRegion *gr, const MFace &face, const std::vector<MVertex*> &ve,
+//                                           std::vector<MVertex*> &veFace, int nPts)
+//{
+//  MElement *incomplete;
+//
+//  fullMatrix<double> points;
+//  int start = getNewFacePointsInFace(face.getNumVertices(), nPts, points);
+//  for (int k = start; k < points.size1(); k++) {
+//    double t1 = points(k, 0);
+//    double t2 = points(k, 1);
+//    SPoint3 pos;
+//    incomplete->pnt(t1, t2, 0, pos);
+//    MVertex* v = new MVertex(pos.x(), pos.y(), pos.z(), gr);
+//    veFace.push_back(v);
+//  }
+//  delete incomplete;
+//}
+
+// Get new interior vertices for a 2D element
+static void getFaceVertices(GFace *gf, MElement *incomplete, MElement *ele,
+                            std::vector<MVertex*> &vf, std::vector<MVertex*> &newHOVert,
+                            faceContainer &faceVertices, bool linear, int nPts = 1)
+{
+  if(gf->geomType() == GEntity::DiscreteSurface ||
+     gf->geomType() == GEntity::BoundaryLayerSurface)
+    linear = true;
+
+  MFace face = ele->getFace(0);
+  std::vector<MVertex*> veFace;
+  if (!linear)                                                                      // Get vertices on geometry if asked...
+    getFaceVerticesOnGeo(gf, incomplete, face, veFace, nPts);
+  else                                                                              // ... otherwise, create from mesh interpolation
+    interpVerticesInExistingFace(gf, face, veFace, nPts);
+  newHOVert.insert(newHOVert.end(), veFace.begin(), veFace.end());
+  faceVertices[face].insert(faceVertices[face].end(), veFace.begin(), veFace.end());
+  vf.insert(vf.end(), veFace.begin(), veFace.end());
+}
+
+// Get new face (excluding edge) vertices for a face of a 3D element
+static void getFaceVertices(GRegion *gr, MElement *incomplete, MElement *ele,
+                            std::vector<MVertex*> &vf, std::vector<MVertex*> &newHOVert,
                             faceContainer &faceVertices, edgeContainer &edgeVertices,
                             bool linear, int nPts = 1)
 {
-  // FIXME: MFace returned by getFace are of order 1
-  // Thus new face vertices are not positioned according to new edge vertices
-  for(int i = 0; i < ele->getNumFaces(); i++){
+  fullMatrix<double> points;
+  int startFace = incomplete ? getNewFacePointsInVolume(incomplete, nPts, points) : 0;
+  int index = startFace;
+  for(int i = 0; i < ele->getNumFaces(); i++) {
     MFace face = ele->getFace(i);
+    int numVert = (face.getNumVertices() == 3) ? nPts * (nPts-1) / 2 : nPts * nPts;
+    std::vector<MVertex*> veFace;
     faceContainer::iterator fIter = faceVertices.find(face);
-    if(fIter != faceVertices.end()) {
+    if(fIter != faceVertices.end()){                                                      // Vertices already exist
       std::vector<MVertex*> vtcs = fIter->second;
-      if(face.getNumVertices() == 3 && nPts > 1){ // tri face
-        int orientation;
-        bool swap;
-        if (fIter->first.computeCorrespondence(face, orientation, swap))
+      int orientation;
+      bool swap;
+      if (fIter->first.computeCorrespondence(face, orientation, swap)) {                  // Check correspondence and apply permutation if needed
+        if(face.getNumVertices() == 3 && nPts > 1)
           reorientTrianglePoints(vtcs, orientation, swap);
-        else
-          Msg::Error("Error in face lookup for recuperation of high order face nodes");
-      }
-      else if(face.getNumVertices() == 4){ // quad face
-        int orientation;
-        bool swap;
-        if (fIter->first.computeCorrespondence(face, orientation, swap)){
+        else if(face.getNumVertices() == 4)
           reorientQuadPoints(vtcs, orientation, swap, nPts-1);
-        }
-        else
-          Msg::Error("Error in face lookup for recuperation of high order face nodes");
       }
-      vf.insert(vf.end(), vtcs.begin(), vtcs.end());
+      else
+        Msg::Error("Error in face lookup for recuperation of high order face nodes");
+      veFace.assign(vtcs.begin(), vtcs.end());
     }
-    else{
-      std::vector<MVertex*> &vtcs = faceVertices[face];
-      fullMatrix<double> points;
-      int start = getNewFacePoints(face.getNumVertices(), nPts, points);
-      if(face.getNumVertices() == 3 && nPts > 1){ // tri face
-        // construct incomplete element to take into account curved
-        // edges on surface boundaries
-        std::vector<MVertex*> hoEdgeNodes;
-        for (int i = 0; i < 3; i++) {
-          MVertex* v0 = face.getVertex(i);
-          MVertex* v1 = face.getVertex((i + 1) % 3);
-          edgeContainer::iterator eIter = edgeVertices.find
-            (std::pair<MVertex*,MVertex*>(std::min(v0, v1), std::max(v0, v1)));
-          if (eIter == edgeVertices.end())
-            Msg::Error("Could not find ho nodes for an edge");
-          if (v0 == eIter->first.first)
-            hoEdgeNodes.insert(hoEdgeNodes.end(), eIter->second.begin(),
-                               eIter->second.end());
-          else
-            hoEdgeNodes.insert(hoEdgeNodes.end(), eIter->second.rbegin(),
-                               eIter->second.rend());
-        }
-        MTriangleN incomplete(face.getVertex(0), face.getVertex(1),
-                              face.getVertex(2), hoEdgeNodes, nPts + 1);
-        for (int k = start; k < points.size1(); k++) {
-          double t1 = points(k, 0);
-          double t2 = points(k, 1);
-          SPoint3 pos;
-          incomplete.pnt(t1, t2, 0, pos);
-          MVertex* v = new MVertex(pos.x(), pos.y(), pos.z(), gr);
-          vtcs.push_back(v);
-          gr->mesh_vertices.push_back(v);
-          vf.push_back(v);
+    else {                                                                                // Vertices do not exist, create them by interpolation
+      if (linear)                                                                         // Interpolate on existing mesh
+        interpVerticesInExistingFace(gr, face, veFace, nPts);
+      else {
+        if (incomplete) {                                                                 // Interpolate in incomplete 3D element if given
+          for(int k = index; k < index + numVert; k++){
+            MVertex *v;
+            const double t1 = points(k, 0);
+            const double t2 = points(k, 1);
+            const double t3 = points(k, 2);
+            SPoint3 pos;
+            incomplete->pnt(t1, t2, t3, pos);
+            v = new MVertex(pos.x(), pos.y(), pos.z(), gr);
+            veFace.push_back(v);
+          }
         }
-      }
-      else if(face.getNumVertices() == 4){ // quad face
-        for (int k = start; k < points.size1(); k++) {
-          double t1 = points(k, 0);
-          double t2 = points(k, 1);
-          SPoint3 pc = face.interpolate(t1, t2);
-          MVertex *v = new MVertex(pc.x(), pc.y(), pc.z(), gr);
-          vtcs.push_back(v);
-          gr->mesh_vertices.push_back(v);
-          vf.push_back(v);
+        else {                                                                            // TODO: Interpolate in incomplete face constructed on the fly
+//          std::vector<MVertex*> &vtcs = faceVertices[face];
+//          fullMatrix<double> points;
+//          int start = getNewFacePointsInFace(face.getNumVertices(), nPts, points);
+//          if(face.getNumVertices() == 3 && nPts > 1){ // tri face
+//            // construct incomplete element to take into account curved
+//            // edges on surface boundaries
+//            std::vector<MVertex*> hoEdgeNodes;
+//            for (int i = 0; i < 3; i++) {
+//              MVertex* v0 = face.getVertex(i);
+//              MVertex* v1 = face.getVertex((i + 1) % 3);
+//              edgeContainer::iterator eIter = edgeVertices.find
+//                (std::pair<MVertex*,MVertex*>(std::min(v0, v1), std::max(v0, v1)));
+//              if (eIter == edgeVertices.end())
+//                Msg::Error("Could not find ho nodes for an edge");
+//              if (v0 == eIter->first.first)
+//                hoEdgeNodes.insert(hoEdgeNodes.end(), eIter->second.begin(),
+//                                   eIter->second.end());
+//              else
+//                hoEdgeNodes.insert(hoEdgeNodes.end(), eIter->second.rbegin(),
+//                                   eIter->second.rend());
+//            }
+//            MTriangleN incomplete(face.getVertex(0), face.getVertex(1),
+//                                  face.getVertex(2), hoEdgeNodes, nPts + 1);
+//            for (int k = start; k < points.size1(); k++) {
+//              double t1 = points(k, 0);
+//              double t2 = points(k, 1);
+//              SPoint3 pos;
+//              incomplete.pnt(t1, t2, 0, pos);
+//              MVertex* v = new MVertex(pos.x(), pos.y(), pos.z(), gr);
+//              veFace.push_back(v);
+//            }
+//          }
+//          else if(face.getNumVertices() == 4){ // quad face
+//            for (int k = start; k < points.size1(); k++) {
+//              double t1 = points(k, 0);
+//              double t2 = points(k, 1);
+//              SPoint3 pc = face.interpolate(t1, t2);
+//              MVertex *v = new MVertex(pc.x(), pc.y(), pc.z(), gr);
+//              veFace.push_back(v);
+//            }
+//          }
+          interpVerticesInExistingFace(gr, face, veFace, nPts);
         }
       }
+      newHOVert.insert(newHOVert.end(), veFace.begin(), veFace.end());
+      faceVertices[face].insert(faceVertices[face].end(), veFace.begin(), veFace.end());
     }
+    vf.insert(vf.end(), veFace.begin(), veFace.end());
+    index += numVert;
   }
 }
 
-static void getFaceAndInteriorVertices(GRegion *gr, MElement *incomplete, MElement *ele,
-                                       std::vector<MVertex*> &vr, faceContainer &faceVertices,
-                                       bool linear, int nPts = 1)
+
+// --------- Creation of high-order volume vertices -----------
+
+static int getNewVolumePoints(MElement *incomplete, int nPts, fullMatrix<double> &points)
 {
-  fullMatrix<double> points;
-  int startFace = 0;
+
   int startInter = 0;
 
   switch (incomplete->getType()){
   case TYPE_TET :
     switch (nPts){
-    case 0: return;
-    case 1: return;
+    case 0:
+    case 1: return -1;
     case 2: points = BasisFactory::getNodalBasis(MSH_TET_20)->points; break;
     case 3: points = BasisFactory::getNodalBasis(MSH_TET_35)->points; break;
     case 4: points = BasisFactory::getNodalBasis(MSH_TET_56)->points; break;
@@ -628,12 +766,11 @@ static void getFaceAndInteriorVertices(GRegion *gr, MElement *incomplete, MEleme
       Msg::Error("getFaceAndInteriorVertices not implemented for order %i", nPts+1);
       break;
     }
-    startFace = 4 + nPts*6;
     startInter = ((nPts+2)*(nPts+3)*(nPts+4)-(nPts-2)*(nPts-1)*(nPts))/6;  // = 4+6*(p-1)+4*(p-2)*(p-1)/2 = 4*(p+1)*(p+2)/2-6*(p-1)-2*4 = 2*p*p+2
     break;
   case TYPE_HEX :
     switch (nPts){
-    case 0: return;
+    case 0: return -1;
     case 1: points = BasisFactory::getNodalBasis(MSH_HEX_27)->points; break;
     case 2: points = BasisFactory::getNodalBasis(MSH_HEX_64)->points; break;
     case 3: points = BasisFactory::getNodalBasis(MSH_HEX_125)->points; break;
@@ -646,12 +783,11 @@ static void getFaceAndInteriorVertices(GRegion *gr, MElement *incomplete, MEleme
       Msg::Error("getFaceAndInteriorVertices not implemented for order %i", nPts+1);
       break;
     }
-    startFace = 8 + nPts*12 ;
     startInter = (nPts+2)*(nPts+2)*(nPts+2) - (nPts)*(nPts)*(nPts) ; // = 6*(p-1)*(p-1)+12*(p-1)+8 = 6*(p+1)*(p+1)-12*(p-1)-2*8 = 6*p*p+2
     break;
   case TYPE_PRI :
     switch (nPts){
-    case 0: return;
+    case 0: return -1;
     case 1: points = BasisFactory::getNodalBasis(MSH_PRI_18)->points; break;
     case 2: points = BasisFactory::getNodalBasis(MSH_PRI_40)->points; break;
     case 3: points = BasisFactory::getNodalBasis(MSH_PRI_75)->points; break;
@@ -664,13 +800,12 @@ static void getFaceAndInteriorVertices(GRegion *gr, MElement *incomplete, MEleme
       Msg::Error("getFaceAndInteriorVertices not implemented for order %i", nPts+1);
       break;
     }
-    startFace = 6 + nPts*9;
     startInter = 4*(nPts+1)*(nPts+1)+2;  // = 4*p*p+2 = 6+9*(p-1)+2*(p-2)*(p-1)/2+3*(p-1)*(p-1) = 2*(p+1)*(p+2)/2+3*(p+1)*(p+1)-9*(p-1)-2*6
     break;
   case TYPE_PYR:
     switch (nPts){
     case 0:
-    case 1: return;
+    case 1: return -1;
     case 2: points = BasisFactory::getNodalBasis(MSH_PYR_30)->points; break;
     case 3: points = BasisFactory::getNodalBasis(MSH_PYR_55)->points; break;
     case 4: points = BasisFactory::getNodalBasis(MSH_PYR_91)->points; break;
@@ -682,55 +817,22 @@ static void getFaceAndInteriorVertices(GRegion *gr, MElement *incomplete, MEleme
       Msg::Error("getFaceAndInteriorVertices not implemented for order %i", nPts+1);
       break;
     }
-    startFace = 5 + nPts*8;
     startInter = ( nPts+2) * ( (nPts+2) + 1) * (2*(nPts+2) + 1) / 6  -
       (nPts-1) * ( (nPts-1) + 1) * (2*(nPts-1) + 1) / 6;
     break;
 
   }
 
-  int index = startFace;
-  for(int i = 0; i < ele->getNumFaces(); i++){
-    MFace face = ele->getFace(i);
-    int numVert = (face.getNumVertices() == 3) ? nPts * (nPts-1) / 2 : nPts * nPts;
-    faceContainer::iterator fIter = faceVertices.find(face);
-    if(fIter != faceVertices.end()) {
-      std::vector<MVertex*> vtcs = fIter->second;
-      if(face.getNumVertices() == 3 && nPts > 1){ // tri face
-        int orientation;
-        bool swap;
-        if (fIter->first.computeCorrespondence(face, orientation, swap))
-          reorientTrianglePoints(vtcs, orientation, swap);
-        else
-          Msg::Error("Error in face lookup for recuperation of high order face nodes");
-      }
-      else if(face.getNumVertices() == 4){ // quad face
-        int orientation;
-        bool swap;
-        if (fIter->first.computeCorrespondence(face, orientation, swap)){
-          reorientQuadPoints(vtcs, orientation, swap, nPts-1);
-        }
-        else
-          Msg::Error("Error in face lookup for recuperation of high order face nodes");
-      }
-      vr.insert(vr.end(), vtcs.begin(), vtcs.end());
-    }
-    else {
-      for(int k = index; k < index + numVert; k++){
-        MVertex *v;
-        const double t1 = points(k, 0);
-        const double t2 = points(k, 1);
-        const double t3 = points(k, 2);
-        SPoint3 pos;
-        incomplete->pnt(t1, t2, t3, pos);
-        v = new MVertex(pos.x(), pos.y(), pos.z(), gr);
-        gr->mesh_vertices.push_back(v);
-        vr.push_back(v);
-      }
-    }
-    index += numVert;
-  }
+  return startInter;
+}
 
+// Get new interior vertices for a 3D element (except pyramid)
+static void getVolumeVertices(GRegion *gr, MElement *incomplete, MElement *ele,
+                              std::vector<MVertex*> &vr,  std::vector<MVertex*> &newHOVert,
+                              bool linear, int nPts = 1)
+{
+  fullMatrix<double> points;
+  int startInter = getNewVolumePoints(incomplete, nPts, points);
   for(int k = startInter; k < points.size1(); k++){
     MVertex *v;
     const double t1 = points(k, 0);
@@ -739,19 +841,138 @@ static void getFaceAndInteriorVertices(GRegion *gr, MElement *incomplete, MEleme
     SPoint3 pos;
     incomplete->pnt(t1, t2, t3, pos);
     v = new MVertex(pos.x(), pos.y(), pos.z(), gr);
-    gr->mesh_vertices.push_back(v);
+    newHOVert.push_back(v);
     vr.push_back(v);
   }
 }
 
-static void setHighOrder(GEdge *ge, edgeContainer &edgeVertices, bool linear,
-                         int nbPts = 1)
+// Get new interior vertices for a pyramid
+static void getVolumeVerticesPyramid(GRegion *gr, MElement *ele, const std::vector<MVertex*> &ve,
+                                     std::vector<MVertex*> &vr, std::vector<MVertex*> &newHOVert,
+                                     bool linear, int nPts = 1)
+{
+  vr.reserve((nPts-1)*(nPts)*(2*(nPts-1)+1)/6);
+  int verts_lvl3[12] = {37,40,38,43,46,44,49,52,50,55,58,56};
+  int verts_lvl2[8];
+  if (nPts == 4) {
+    verts_lvl2[0] = 42; verts_lvl2[1] = 41;
+    verts_lvl2[2] = 48; verts_lvl2[3] = 47;
+    verts_lvl2[4] = 54; verts_lvl2[5] = 53;
+    verts_lvl2[6] = 60; verts_lvl2[7] = 59;
+  }
+  else {
+    verts_lvl2[0] = 29; verts_lvl2[1] = 30;
+    verts_lvl2[2] = 35; verts_lvl2[3] = 36;
+    verts_lvl2[4] = 38; verts_lvl2[5] = 39;
+    verts_lvl2[6] = 32; verts_lvl2[7] = 33;
+  }
+  int verts_lvl1[4];
+  switch(nPts) {
+  case(4):
+    verts_lvl1[0] = 39;
+    verts_lvl1[1] = 45;
+    verts_lvl1[2] = 51;
+    verts_lvl1[3] = 57;
+    break;
+  case(3):
+    verts_lvl1[0] = 31;
+    verts_lvl1[1] = 37;
+    verts_lvl1[2] = 40;
+    verts_lvl1[3] = 34;
+    break;
+  case(2):
+    verts_lvl1[0] = 21;
+    verts_lvl1[1] = 23;
+    verts_lvl1[2] = 24;
+    verts_lvl1[3] = 22;
+    break;
+  }
+  for (int q = 0; q < nPts - 1; q++) {
+    std::vector<MVertex*> vq, veq;
+    vq.push_back(ve[2*nPts + q]);
+    vq.push_back(ve[4*nPts + q]);
+    vq.push_back(ve[6*nPts + q]);
+    vq.push_back(ve[7*nPts + q]);
+
+    if (nPts-q == 4)
+      for (int f = 0; f < 12; f++)
+        veq.push_back(ve[verts_lvl3[f]-5]);
+    else if (nPts-q == 3)
+      for (int f = 0; f < 8; f++)
+        veq.push_back(ve[verts_lvl2[f]-5]);
+    else if (nPts-q == 2)
+      for (int f = 0; f < 4; f++)
+        veq.push_back(ve[verts_lvl1[f]-5]);
+
+    if (nPts-q == 2) {
+      MQuadrangle8 incpl2(vq[0], vq[1], vq[2], vq[3],
+                          veq[0], veq[1], veq[2], veq[3]);
+      SPoint3 pointz;
+      incpl2.pnt(0,0,0,pointz);
+      MVertex *v = new MVertex(pointz.x(), pointz.y(), pointz.z(), gr);
+      newHOVert.push_back(v);
+      std::vector<MVertex*>::iterator cursor = vr.begin();
+      cursor += nPts == 2 ? 0 : 4;
+      vr.insert(cursor, v);
+    }
+    else if (nPts-q == 3) {
+      MQuadrangleN incpl2(vq[0], vq[1], vq[2], vq[3], veq, 3);
+      int offsets[4] = {nPts == 4 ? 7 : 0,
+                        nPts == 4 ? 9 : 1,
+                        nPts == 4 ? 11 : 2,
+                        nPts == 4 ? 12 : 3};
+      double quad_v [4][2] = {{-1.0/3.0, -1.0/3.0},
+                              { 1.0/3.0, -1.0/3.0},
+                              { 1.0/3.0,  1.0/3.0},
+                              {-1.0/3.0,  1.0/3.0}};
+      SPoint3 pointz;
+      for (int k = 0; k<4; k++) {
+        incpl2.pnt(quad_v[k][0], quad_v[k][1], 0, pointz);
+        MVertex *v = new MVertex(pointz.x(), pointz.y(), pointz.z(), gr);
+        newHOVert.push_back(v);
+        std::vector<MVertex*>::iterator cursor = vr.begin();
+        cursor += offsets[k];
+        vr.insert(cursor, v);
+      }
+    }
+    else if (nPts-q == 4) {
+      MQuadrangleN incpl2(vq[0], vq[1], vq[2], vq[3], veq, 4);
+      int offsets[9] = {0, 1, 2, 3, 5, 8, 10, 6, 13};
+      double quad_v [9][2] = {
+        { -0.5, -0.5},
+        {  0.5, -0.5},
+        {  0.5,  0.5},
+        { -0.5,  0.5},
+        {  0.0, -0.5},
+        {  0.5,  0.0},
+        {  0.0,  0.5},
+        { -0.5,  0.0},
+        {  0.0,  0.0}
+      };
+      SPoint3 pointz;
+      for (int k = 0; k<9; k++) {
+        incpl2.pnt(quad_v[k][0], quad_v[k][1], 0, pointz);
+        MVertex *v = new MVertex(pointz.x(), pointz.y(), pointz.z(), gr);
+        newHOVert.push_back(v);
+        std::vector<MVertex*>::iterator cursor = vr.begin();
+        cursor += offsets[k];
+        vr.insert(cursor, v);
+      }
+    }
+  }
+}
+
+
+// --------- Creation of high-order elements -----------
+
+static void setHighOrder(GEdge *ge, std::vector<MVertex*> &newHOVert,
+                         edgeContainer &edgeVertices, bool linear, int nbPts = 1)
 {
   std::vector<MLine*> lines2;
   for(unsigned int i = 0; i < ge->lines.size(); i++){
     MLine *l = ge->lines[i];
     std::vector<MVertex*> ve;
-    getEdgeVertices(ge, l, ve, edgeVertices, linear, nbPts);
+    getEdgeVertices(ge, l, ve, newHOVert, edgeVertices, linear, nbPts);
     if(nbPts == 1)
       lines2.push_back(new MLine3(l->getVertex(0), l->getVertex(1), ve[0], l->getPartition()));
     else
@@ -762,13 +983,13 @@ static void setHighOrder(GEdge *ge, edgeContainer &edgeVertices, bool linear,
   ge->deleteVertexArrays();
 }
 
-static MTriangle *setHighOrder(MTriangle *t, GFace *gf,
+static MTriangle *setHighOrder(MTriangle *t, GFace *gf, std::vector<MVertex*> &newHOVert,
                                edgeContainer &edgeVertices,
                                faceContainer &faceVertices,
                                bool linear, bool incomplete, int nPts)
 {
   std::vector<MVertex*> ve, vf;
-  getEdgeVertices(gf, t, ve, edgeVertices, linear, nPts);
+  getEdgeVertices(gf, t, ve, newHOVert, edgeVertices, linear, nPts);
   if(nPts == 1){
     return new MTriangle6(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                           ve[0], ve[1], ve[2], 0, t->getPartition());
@@ -777,7 +998,7 @@ static MTriangle *setHighOrder(MTriangle *t, GFace *gf,
     if(!incomplete){
       MTriangleN incpl(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                        ve, nPts + 1, 0, t->getPartition());
-      getFaceVertices(gf, &incpl, t, vf, faceVertices, linear, nPts);
+      getFaceVertices(gf, &incpl, t, vf, newHOVert, faceVertices, linear, nPts);
       ve.insert(ve.end(), vf.begin(), vf.end());
     }
     return new MTriangleN(t->getVertex(0), t->getVertex(1), t->getVertex(2),
@@ -785,13 +1006,13 @@ static MTriangle *setHighOrder(MTriangle *t, GFace *gf,
   }
 }
 
-static MQuadrangle *setHighOrder(MQuadrangle *q, GFace *gf,
+static MQuadrangle *setHighOrder(MQuadrangle *q, GFace *gf, std::vector<MVertex*> &newHOVert,
                                  edgeContainer &edgeVertices,
                                  faceContainer &faceVertices,
                                  bool linear, bool incomplete, int nPts)
 {
   std::vector<MVertex*> ve, vf;
-  getEdgeVertices(gf, q, ve, edgeVertices, linear, nPts);
+  getEdgeVertices(gf, q, ve, newHOVert, edgeVertices, linear, nPts);
   if(incomplete){
     if(nPts == 1){
       return new MQuadrangle8(q->getVertex(0), q->getVertex(1), q->getVertex(2),
@@ -807,7 +1028,7 @@ static MQuadrangle *setHighOrder(MQuadrangle *q, GFace *gf,
   else {
     MQuadrangleN incpl(q->getVertex(0), q->getVertex(1), q->getVertex(2),
                        q->getVertex(3), ve, nPts + 1, 0, q->getPartition());
-    getFaceVertices(gf, &incpl, q, vf, faceVertices, linear, nPts);
+    getFaceVertices(gf, &incpl, q, vf, newHOVert, faceVertices, linear, nPts);
     ve.insert(ve.end(), vf.begin(), vf.end());
     if(nPts == 1){
       return new MQuadrangle9(q->getVertex(0), q->getVertex(1), q->getVertex(2),
@@ -822,15 +1043,15 @@ static MQuadrangle *setHighOrder(MQuadrangle *q, GFace *gf,
   }
 }
 
-static void setHighOrder(GFace *gf, edgeContainer &edgeVertices,
-                         faceContainer &faceVertices, bool linear, bool incomplete,
-                         int nPts = 1)
+static void setHighOrder(GFace *gf, std::vector<MVertex*> &newHOVert,
+                         edgeContainer &edgeVertices, faceContainer &faceVertices,
+                         bool linear, bool incomplete, int nPts = 1)
 {
   std::vector<MTriangle*> triangles2;
   for(unsigned int i = 0; i < gf->triangles.size(); i++){
     MTriangle *t = gf->triangles[i];
-    MTriangle *tNew = setHighOrder(t, gf, edgeVertices, faceVertices, linear,
-                                   incomplete, nPts);
+    MTriangle *tNew = setHighOrder(t, gf, newHOVert, edgeVertices,
+                                   faceVertices, linear, incomplete, nPts);
     triangles2.push_back(tNew);
     delete t;
   }
@@ -838,8 +1059,8 @@ static void setHighOrder(GFace *gf, edgeContainer &edgeVertices,
   std::vector<MQuadrangle*> quadrangles2;
   for(unsigned int i = 0; i < gf->quadrangles.size(); i++){
     MQuadrangle *q = gf->quadrangles[i];
-    MQuadrangle *qNew = setHighOrder(q, gf, edgeVertices, faceVertices, linear,
-                                     incomplete, nPts);
+    MQuadrangle *qNew = setHighOrder(q, gf, newHOVert, edgeVertices,
+                                     faceVertices, linear, incomplete, nPts);
     quadrangles2.push_back(qNew);
     delete q;
   }
@@ -848,12 +1069,13 @@ static void setHighOrder(GFace *gf, edgeContainer &edgeVertices,
 }
 
 static MTetrahedron *setHighOrder(MTetrahedron *t, GRegion *gr,
+                                  std::vector<MVertex*> &newHOVert,
                                   edgeContainer &edgeVertices,
                                   faceContainer &faceVertices,
                                   bool linear, bool incomplete, int nPts)
 {
   std::vector<MVertex*> ve, vf, vr;
-  getEdgeVertices(gr, t, ve, edgeVertices, linear, nPts);
+  getEdgeVertices(gr, t, ve, newHOVert, edgeVertices, linear, nPts);
   if(nPts == 1){
     return new MTetrahedron10(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                               t->getVertex(3), ve[0], ve[1], ve[2], ve[3], ve[4], ve[5],
@@ -867,8 +1089,10 @@ static MTetrahedron *setHighOrder(MTetrahedron *t, GRegion *gr,
       // it either way)
       MTetrahedronN incpl(t->getVertex(0), t->getVertex(1), t->getVertex(2),
                           t->getVertex(3), ve, nPts + 1, 0, t->getPartition());
-      getFaceAndInteriorVertices(gr, &incpl, t, vf, faceVertices, linear, nPts);
+      getFaceVertices(gr, &incpl, t, vf, newHOVert, faceVertices, edgeVertices, linear, nPts);
       ve.insert(ve.end(), vf.begin(), vf.end());
+      getVolumeVertices(gr, &incpl, t, vr, newHOVert, linear, nPts);
+      ve.insert(ve.end(), vr.begin(), vr.end());
     }
     return new MTetrahedronN(t->getVertex(0), t->getVertex(1),
                              t->getVertex(2), t->getVertex(3), ve, nPts + 1,
@@ -877,12 +1101,13 @@ static MTetrahedron *setHighOrder(MTetrahedron *t, GRegion *gr,
 }
 
 static MHexahedron *setHighOrder(MHexahedron *h, GRegion *gr,
+                                 std::vector<MVertex*> &newHOVert,
                                  edgeContainer &edgeVertices,
                                  faceContainer &faceVertices,
                                  bool linear, bool incomplete, int nPts)
 {
   std::vector<MVertex*> ve, vf, vr;
-  getEdgeVertices(gr, h, ve, edgeVertices, linear, nPts);
+  getEdgeVertices(gr, h, ve, newHOVert, edgeVertices, linear, nPts);
   if(incomplete){
     if(nPts == 1){
       return new MHexahedron20(h->getVertex(0), h->getVertex(1), h->getVertex(2),
@@ -909,12 +1134,13 @@ static MHexahedron *setHighOrder(MHexahedron *h, GRegion *gr,
                           h->getVertex(6), h->getVertex(7), ve[0], ve[1], ve[2],
                           ve[3], ve[4], ve[5], ve[6], ve[7], ve[8], ve[9], ve[10],
                           ve[11], 0, h->getPartition());
-      getFaceAndInteriorVertices(gr, &incpl, h, vf, faceVertices, linear, nPts);
+      getFaceVertices(gr, &incpl, h, vf, newHOVert, faceVertices, edgeVertices, linear, nPts);
+      getVolumeVertices(gr, &incpl, h, vr, newHOVert, linear, nPts);
       return new MHexahedron27(h->getVertex(0), h->getVertex(1), h->getVertex(2),
                                h->getVertex(3), h->getVertex(4), h->getVertex(5),
                                h->getVertex(6), h->getVertex(7), ve[0], ve[1], ve[2],
                                ve[3], ve[4], ve[5], ve[6], ve[7], ve[8], ve[9], ve[10],
-                               ve[11], vf[0], vf[1], vf[2], vf[3], vf[4], vf[5], vf[6],
+                               ve[11], vf[0], vf[1], vf[2], vf[3], vf[4], vf[5], vr[0],
                                0, h->getPartition());
     }
     else {
@@ -922,8 +1148,10 @@ static MHexahedron *setHighOrder(MHexahedron *h, GRegion *gr,
                          h->getVertex(3), h->getVertex(4), h->getVertex(5),
                          h->getVertex(6), h->getVertex(7), ve, nPts + 1, 0,
                          h->getPartition());
-      getFaceAndInteriorVertices(gr, &incpl, h, vf, faceVertices, linear, nPts);
+      getFaceVertices(gr, &incpl, h, vf, newHOVert, faceVertices, edgeVertices, linear, nPts);
       ve.insert(ve.end(), vf.begin(), vf.end());
+      getVolumeVertices(gr, &incpl, h, vr, newHOVert, linear, nPts);
+      ve.insert(ve.end(), vr.begin(), vr.end());
       return new MHexahedronN(h->getVertex(0), h->getVertex(1), h->getVertex(2),
                               h->getVertex(3), h->getVertex(4), h->getVertex(5),
                               h->getVertex(6), h->getVertex(7), ve, nPts + 1,
@@ -933,12 +1161,13 @@ static MHexahedron *setHighOrder(MHexahedron *h, GRegion *gr,
 }
 
 static MPrism *setHighOrder(MPrism *p, GRegion *gr,
+                            std::vector<MVertex*> &newHOVert,
                             edgeContainer &edgeVertices,
                             faceContainer &faceVertices,
                             bool linear, bool incomplete, int nPts)
 {
   std::vector<MVertex*> ve, vf, vr;
-  getEdgeVertices(gr, p, ve, edgeVertices, linear, nPts);
+  getEdgeVertices(gr, p, ve, newHOVert, edgeVertices, linear, nPts);
   if(incomplete){
     if(nPts == 1){
       return new MPrism15(p->getVertex(0), p->getVertex(1), p->getVertex(2),
@@ -960,8 +1189,7 @@ static MPrism *setHighOrder(MPrism *p, GRegion *gr,
     MPrismN incpl(p->getVertex(0), p->getVertex(1), p->getVertex(2),
                   p->getVertex(3), p->getVertex(4), p->getVertex(5),
                   ve, nPts + 1, 0, p->getPartition());
-    getFaceAndInteriorVertices(gr, &incpl, p, vf, faceVertices, linear, nPts);
-
+    getFaceVertices(gr, &incpl, p, vf, newHOVert, faceVertices, edgeVertices, linear, nPts);
     if (nPts == 1) {
       return new MPrism18(p->getVertex(0), p->getVertex(1), p->getVertex(2),
                           p->getVertex(3), p->getVertex(4), p->getVertex(5),
@@ -971,6 +1199,8 @@ static MPrism *setHighOrder(MPrism *p, GRegion *gr,
     }
     else {
       ve.insert(ve.end(), vf.begin(), vf.end());
+      getVolumeVertices(gr, &incpl, p, vr, newHOVert, linear, nPts);
+      ve.insert(ve.end(), vr.begin(), vr.end());
       return new MPrismN(p->getVertex(0), p->getVertex(1), p->getVertex(2),
                          p->getVertex(3), p->getVertex(4), p->getVertex(5),
                          ve, nPts + 1, 0, p->getPartition());
@@ -979,124 +1209,19 @@ static MPrism *setHighOrder(MPrism *p, GRegion *gr,
 }
 
 static MPyramid *setHighOrder(MPyramid *p, GRegion *gr,
+                              std::vector<MVertex*> &newHOVert,
                               edgeContainer &edgeVertices,
                               faceContainer &faceVertices,
                               bool linear, bool incomplete, int nPts)
 {
   std::vector<MVertex*> ve, vf, vr;
-  getEdgeVertices(gr, p, ve, edgeVertices, linear, nPts);
-  getFaceVertices(gr, p, vf, faceVertices, edgeVertices, linear, nPts);
+  getEdgeVertices(gr, p, ve, newHOVert, edgeVertices, linear, nPts);
+//  MPyramidN incpl(p->getVertex(0), p->getVertex(1), p->getVertex(2),
+//                  p->getVertex(3), p->getVertex(4), ve, nPts + 1, 0, p->getPartition());
+//  getFaceVertices(gr, &incpl, p, vf, faceVertices, edgeVertices, linear, nPts);
+  getFaceVertices(gr, 0, p, vf, newHOVert, faceVertices, edgeVertices, linear, nPts);
   ve.insert(ve.end(), vf.begin(), vf.end());
-  vr.reserve((nPts-1)*(nPts)*(2*(nPts-1)+1)/6);
-  int verts_lvl3[12] = {37,40,38,43,46,44,49,52,50,55,58,56};
-  int verts_lvl2[8];
-  if (nPts == 4) {
-    verts_lvl2[0] = 42; verts_lvl2[1] = 41;
-    verts_lvl2[2] = 48; verts_lvl2[3] = 47;
-    verts_lvl2[4] = 54; verts_lvl2[5] = 53;
-    verts_lvl2[6] = 60; verts_lvl2[7] = 59;
-  }
-  else {
-    verts_lvl2[0] = 29; verts_lvl2[1] = 30;
-    verts_lvl2[2] = 35; verts_lvl2[3] = 36;
-    verts_lvl2[4] = 38; verts_lvl2[5] = 39;
-    verts_lvl2[6] = 32; verts_lvl2[7] = 33;
-  }
-  int verts_lvl1[4];
-  switch(nPts) {
-  case(4):
-    verts_lvl1[0] = 39;
-    verts_lvl1[1] = 45;
-    verts_lvl1[2] = 51;
-    verts_lvl1[3] = 57;
-    break;
-  case(3):
-    verts_lvl1[0] = 31;
-    verts_lvl1[1] = 37;
-    verts_lvl1[2] = 40;
-    verts_lvl1[3] = 34;
-    break;
-  case(2):
-    verts_lvl1[0] = 21;
-    verts_lvl1[1] = 23;
-    verts_lvl1[2] = 24;
-    verts_lvl1[3] = 22;
-    break;
-  }
-  for (int q = 0; q < nPts - 1; q++) {
-    std::vector<MVertex*> vq, veq;
-    vq.push_back(ve[2*nPts + q]);
-    vq.push_back(ve[4*nPts + q]);
-    vq.push_back(ve[6*nPts + q]);
-    vq.push_back(ve[7*nPts + q]);
-
-    if (nPts-q == 4)
-      for (int f = 0; f < 12; f++)
-        veq.push_back(ve[verts_lvl3[f]-5]);
-    else if (nPts-q == 3)
-      for (int f = 0; f < 8; f++)
-        veq.push_back(ve[verts_lvl2[f]-5]);
-    else if (nPts-q == 2)
-      for (int f = 0; f < 4; f++)
-        veq.push_back(ve[verts_lvl1[f]-5]);
-
-    if (nPts-q == 2) {
-      MQuadrangle8 incpl2(vq[0], vq[1], vq[2], vq[3],
-                          veq[0], veq[1], veq[2], veq[3]);
-      SPoint3 pointz;
-      incpl2.pnt(0,0,0,pointz);
-      MVertex *v = new MVertex(pointz.x(), pointz.y(), pointz.z(), gr);
-
-      gr->mesh_vertices.push_back(v);
-      std::vector<MVertex*>::iterator cursor = vr.begin();
-      cursor += nPts == 2 ? 0 : 4;
-      vr.insert(cursor, v);
-    }
-    else if (nPts-q == 3) {
-      MQuadrangleN incpl2(vq[0], vq[1], vq[2], vq[3], veq, 3);
-      int offsets[4] = {nPts == 4 ? 7 : 0,
-                        nPts == 4 ? 9 : 1,
-                        nPts == 4 ? 11 : 2,
-                        nPts == 4 ? 12 : 3};
-      double quad_v [4][2] = {{-1.0/3.0, -1.0/3.0},
-                              { 1.0/3.0, -1.0/3.0},
-                              { 1.0/3.0,  1.0/3.0},
-                              {-1.0/3.0,  1.0/3.0}};
-      SPoint3 pointz;
-      for (int k = 0; k<4; k++) {
-        incpl2.pnt(quad_v[k][0], quad_v[k][1], 0, pointz);
-        MVertex *v = new MVertex(pointz.x(), pointz.y(), pointz.z(), gr);
-        gr->mesh_vertices.push_back(v);
-        std::vector<MVertex*>::iterator cursor = vr.begin();
-        cursor += offsets[k];
-        vr.insert(cursor, v);
-      }
-    }
-    else if (nPts-q == 4) {
-      MQuadrangleN incpl2(vq[0], vq[1], vq[2], vq[3], veq, 4);
-      int offsets[9] = {0, 1, 2, 3, 5, 8, 10, 6, 13};
-      double quad_v [9][2] = {
-        { -0.5, -0.5},
-        {  0.5, -0.5},
-        {  0.5,  0.5},
-        { -0.5,  0.5},
-        {  0.0, -0.5},
-        {  0.5,  0.0},
-        {  0.0,  0.5},
-        { -0.5,  0.0},
-        {  0.0,  0.0}
-      };
-      SPoint3 pointz;
-      for (int k = 0; k<9; k++) {
-        incpl2.pnt(quad_v[k][0], quad_v[k][1], 0, pointz);
-        MVertex *v = new MVertex(pointz.x(), pointz.y(), pointz.z(), gr);
-        gr->mesh_vertices.push_back(v);
-        std::vector<MVertex*>::iterator cursor = vr.begin();
-        cursor += offsets[k];
-        vr.insert(cursor, v);
-      }
-    }
-  }
+  getVolumeVerticesPyramid(gr, p, ve, vr, newHOVert, linear, nPts);
   ve.insert(ve.end(), vr.begin(), vr.end());
   return new MPyramidN(p->getVertex(0), p->getVertex(1),
                        p->getVertex(2), p->getVertex(3),
@@ -1104,15 +1229,15 @@ static MPyramid *setHighOrder(MPyramid *p, GRegion *gr,
                        0, p->getPartition());
 }
 
-static void setHighOrder(GRegion *gr, edgeContainer &edgeVertices,
-                         faceContainer &faceVertices, bool linear, bool incomplete,
-                         int nPts = 1)
+static void setHighOrder(GRegion *gr, std::vector<MVertex*> &newHOVert,
+                         edgeContainer &edgeVertices, faceContainer &faceVertices,
+                         bool linear, bool incomplete, int nPts = 1)
 {
   std::vector<MTetrahedron*> tetrahedra2;
   for(unsigned int i = 0; i < gr->tetrahedra.size(); i++){
     MTetrahedron *t = gr->tetrahedra[i];
-    MTetrahedron *tNew = setHighOrder(t, gr, edgeVertices, faceVertices, linear,
-                                      incomplete, nPts);
+    MTetrahedron *tNew = setHighOrder(t, gr, newHOVert, edgeVertices,
+                                      faceVertices, linear, incomplete, nPts);
     tetrahedra2.push_back(tNew);
     delete t;
   }
@@ -1121,8 +1246,8 @@ static void setHighOrder(GRegion *gr, edgeContainer &edgeVertices,
   std::vector<MHexahedron*> hexahedra2;
   for(unsigned int i = 0; i < gr->hexahedra.size(); i++){
     MHexahedron *h = gr->hexahedra[i];
-    MHexahedron *hNew = setHighOrder(h, gr, edgeVertices, faceVertices, linear,
-                                     incomplete, nPts);
+    MHexahedron *hNew = setHighOrder(h, gr, newHOVert, edgeVertices,
+                                     faceVertices, linear, incomplete, nPts);
     hexahedra2.push_back(hNew);
     delete h;
   }
@@ -1131,8 +1256,8 @@ static void setHighOrder(GRegion *gr, edgeContainer &edgeVertices,
   std::vector<MPrism*> prisms2;
   for(unsigned int i = 0; i < gr->prisms.size(); i++){
     MPrism *p = gr->prisms[i];
-    MPrism *pNew = setHighOrder(p, gr, edgeVertices, faceVertices, linear,
-                                incomplete, nPts);
+    MPrism *pNew = setHighOrder(p, gr, newHOVert, edgeVertices,
+                                faceVertices, linear, incomplete, nPts);
     prisms2.push_back(pNew);
     delete p;
   }
@@ -1141,8 +1266,8 @@ static void setHighOrder(GRegion *gr, edgeContainer &edgeVertices,
   std::vector<MPyramid*> pyramids2;
   for(unsigned int i = 0; i < gr->pyramids.size(); i++) {
     MPyramid *p = gr->pyramids[i];
-    MPyramid *pNew = setHighOrder(p, gr, edgeVertices, faceVertices, linear,
-                                  incomplete, nPts);
+    MPyramid *pNew = setHighOrder(p, gr, newHOVert, edgeVertices,
+                                  faceVertices, linear, incomplete, nPts);
     pyramids2.push_back(pNew);
     delete p;
   }
@@ -1151,6 +1276,9 @@ static void setHighOrder(GRegion *gr, edgeContainer &edgeVertices,
   gr->deleteVertexArrays();
 }
 
+
+// --------- High-level functions -----------
+
 template<class T>
 static void setFirstOrder(GEntity *e, std::vector<T*> &elements, bool onlyVisible)
 {
@@ -1169,7 +1297,8 @@ static void setFirstOrder(GEntity *e, std::vector<T*> &elements, bool onlyVisibl
   e->deleteVertexArrays();
 }
 
-static void removeHighOrderVertices(GEntity *e, bool onlyVisible)
+static void updateHighOrderVertices(GEntity *e,
+                                    const std::vector<MVertex*> &newHOVert, bool onlyVisible)
 {
   if (onlyVisible && !e->getVisibility())return;
   std::vector<MVertex*> v1;
@@ -1179,7 +1308,9 @@ static void removeHighOrderVertices(GEntity *e, bool onlyVisible)
     else
       v1.push_back(e->mesh_vertices[i]);
   }
+  v1.insert(v1.end(), newHOVert.begin(), newHOVert.end());
   e->mesh_vertices = v1;
+  e->deleteVertexArrays();
 }
 
 void SetOrder1(GModel *m,  bool onlyVisible)
@@ -1202,12 +1333,13 @@ void SetOrder1(GModel *m,  bool onlyVisible)
   }
 
   // remove all high order vertices
+  std::vector<MVertex*> newHOVert;
   for(GModel::eiter it = m->firstEdge(); it != m->lastEdge(); ++it)
-    removeHighOrderVertices(*it, onlyVisible);
+    updateHighOrderVertices(*it, newHOVert, onlyVisible);
   for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it)
-    removeHighOrderVertices(*it, onlyVisible);
+    updateHighOrderVertices(*it, newHOVert, onlyVisible);
   for(GModel::riter it = m->firstRegion(); it != m->lastRegion(); ++it)
-    removeHighOrderVertices(*it, onlyVisible);
+    updateHighOrderVertices(*it, newHOVert, onlyVisible);
 }
 
 void checkHighOrderTriangles(const char* cc, GModel *m,
@@ -1334,12 +1466,12 @@ void SetOrderN(GModel *m, int order, bool linear, bool incomplete, bool onlyVisi
 
   double t1 = Cpu();
 
-  // first, make sure to remove any existsing second order vertices/elements
-  SetOrder1(m, onlyVisible);
+  m->destroyMeshCaches();
 
-  // then create new second order vertices/elements
+  // Keep track of vertex/entities created
   edgeContainer edgeVertices;
   faceContainer faceVertices;
+  std::map<GEntity*,std::vector<MVertex*> > newHOVert;
 
   Msg::ResetProgressMeter();
 
@@ -1349,23 +1481,31 @@ void SetOrderN(GModel *m, int order, bool linear, bool incomplete, bool onlyVisi
     Msg::Info("Meshing curve %d order %d", (*it)->tag(), order);
     Msg::ProgressMeter(++counter, nTot, false, msg);
     if (onlyVisible && !(*it)->getVisibility()) continue;
-    setHighOrder(*it, edgeVertices, linear, nPts);
+    setHighOrder(*it, newHOVert[*it], edgeVertices, linear, nPts);
   }
 
   for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it) {
     Msg::Info("Meshing surface %d order %d", (*it)->tag(), order);
     Msg::ProgressMeter(++counter, nTot, false, msg);
     if (onlyVisible && !(*it)->getVisibility()) continue;
-    setHighOrder(*it, edgeVertices, faceVertices, linear, incomplete, nPts);
+    setHighOrder(*it, newHOVert[*it], edgeVertices, faceVertices, linear, incomplete, nPts);
   }
 
   for(GModel::riter it = m->firstRegion(); it != m->lastRegion(); ++it) {
     Msg::Info("Meshing volume %d order %d", (*it)->tag(), order);
     Msg::ProgressMeter(++counter, nTot, false, msg);
     if (onlyVisible && !(*it)->getVisibility())continue;
-    setHighOrder(*it, edgeVertices, faceVertices, linear, incomplete, nPts);
+    setHighOrder(*it, newHOVert[*it], edgeVertices, faceVertices, linear, incomplete, nPts);
   }
 
+  // Update all high order vertices
+  for(GModel::eiter it = m->firstEdge(); it != m->lastEdge(); ++it)
+    updateHighOrderVertices(*it, newHOVert[*it], onlyVisible);
+  for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it)
+    updateHighOrderVertices(*it, newHOVert[*it], onlyVisible);
+  for(GModel::riter it = m->firstRegion(); it != m->lastRegion(); ++it)
+    updateHighOrderVertices(*it, newHOVert[*it], onlyVisible);
+
   double t2 = Cpu();
 
   std::vector<MElement*> bad;
@@ -1396,6 +1536,7 @@ void SetHighOrderComplete(GModel *m, bool onlyVisible)
   faceContainer faceVertices;
   for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it) {
     if (onlyVisible && !(*it)->getVisibility()) continue;
+    std::vector<MVertex*> dumNewHOVert;
     std::vector<MTriangle*> newT;
     std::vector<MQuadrangle*> newQ;
     for (unsigned int i = 0; i < (*it)->triangles.size(); i++){
@@ -1403,7 +1544,7 @@ void SetHighOrderComplete(GModel *m, bool onlyVisible)
       std::vector<MVertex*> vf, vt;
       int nPts = t->getPolynomialOrder() - 1;
       MTriangle TEMP (t->getVertex(0), t->getVertex(1), t->getVertex(2), 0, t->getPartition());
-      getFaceVertices (*it, t, t, vf, faceVertices, false, nPts);
+      getFaceVertices (*it, t, t, vf, dumNewHOVert, faceVertices, false, nPts);
       for (int j=3;j<t->getNumVertices();j++)vt.push_back(t->getVertex(j));
       vt.insert(vt.end(), vf.begin(), vf.end());
       MTriangleN *newTr = new MTriangleN(t->getVertex(0), t->getVertex(1), t->getVertex(2),
@@ -1420,7 +1561,7 @@ void SetHighOrderComplete(GModel *m, bool onlyVisible)
       int nPts = t->getPolynomialOrder() - 1;
       MQuadrangle TEMP (t->getVertex(0), t->getVertex(1), t->getVertex(2),
                         t->getVertex(3), 0, t->getPartition());
-      getFaceVertices (*it, t, &TEMP, vf, faceVertices, false, nPts);
+      getFaceVertices (*it, t, &TEMP, vf, dumNewHOVert, faceVertices, false, nPts);
       for (int j=4;j<t->getNumVertices();j++)vt.push_back(t->getVertex(j));
       vt.insert(vt.end(), vf.begin(), vf.end());
       newQ.push_back(new MQuadrangleN(t->getVertex(0), t->getVertex(1),