diff --git a/Geo/GEdge.cpp b/Geo/GEdge.cpp
index 5db8f3c3f7b938b62f9b7e80a9ebe0b164c8aeda..8336df7664c3848e2cdefa342ef9d49138a15d29 100644
--- a/Geo/GEdge.cpp
+++ b/Geo/GEdge.cpp
@@ -96,8 +96,10 @@ void GEdge::setMeshMaster(GEdge* ge,const std::vector<double>& tfo)
   SVector3 d01 = locXYZ0 - tfoXYZ1;
   SVector3 d11 = locXYZ1 - tfoXYZ1;
 
-  double tol = CTX::instance()->geom.tolerance;
-
+  double tol = CTX::instance()->geom.tolerance * CTX::instance()->lc;
+  
+  bool fwd = (d00.norm()*d11.norm() < d01.norm()*d10.norm());
+  
   if ((d00.norm() < tol) && (d11.norm() < tol)) {
     GEntity::setMeshMaster(ge,tfo);
     masterOrientation = 1;
@@ -107,6 +109,7 @@ void GEdge::setMeshMaster(GEdge* ge,const std::vector<double>& tfo)
     getEndVertex()  ->setMeshMaster(ge->getEndVertex()  ,tfo);
     return;
   }
+    
   if ((d01.norm() < tol) && (d10.norm() < tol)) {
     GEntity::setMeshMaster(ge,tfo);
     masterOrientation = -1;
@@ -116,10 +119,15 @@ void GEdge::setMeshMaster(GEdge* ge,const std::vector<double>& tfo)
     getEndVertex()  ->setMeshMaster(ge->getBeginVertex(),tfo);
     return;
   }
-
-  Msg::Error("Transformation from edge %d (%d-%d) to %d (%d-%d) is incorrect",
-             ge->tag(),ge->getBeginVertex()->tag(),ge->getEndVertex()->tag(),
-             this->tag(),this->getBeginVertex()->tag(),this->getEndVertex()->tag());
+  
+  Msg::Info("Error in transformation from edge %d (%d-%d) to %d (%d-%d)"
+            "(minimal transformed node distances %g %g, tolerance %g)",
+            ge->tag(),ge->getBeginVertex()->tag(),ge->getEndVertex()->tag(),
+            this->tag(),
+            this->getBeginVertex()->tag(),
+            this->getEndVertex()->tag(),
+            fwd ? d00.norm() : d01.norm(),
+            fwd ? d11.norm() : d10.norm(),tol);
 }
 
 void GEdge::reverse()
diff --git a/Geo/GEntity.cpp b/Geo/GEntity.cpp
index fdbcae3f66cb73ed913791f94e77b02f18902ae7..1a504f083359dd89c8bb057ec73bd75a6e67b9e1 100644
--- a/Geo/GEntity.cpp
+++ b/Geo/GEntity.cpp
@@ -103,3 +103,67 @@ void GEntity::setMeshMaster(GEntity* gMaster,const std::vector<double>& tfo)
 
 // gets the entity from which the mesh will be copied
 GEntity* GEntity::meshMaster() const { return _meshMaster; }
+
+void GEntity::updateVertices(const std::map<MVertex*,MVertex*>& old2new) {
+
+  // update the list of the vertices
+
+  std::vector<MVertex*> newMeshVertices;
+  std::vector<MVertex*>::iterator mIter = mesh_vertices.begin();
+  for (;mIter!=mesh_vertices.end();++mIter) {
+
+    MVertex* vtx = *mIter;
+    std::map<MVertex*,MVertex*>::const_iterator cIter = old2new.find(vtx);
+    if (cIter!=old2new.end()) vtx = cIter->second;
+    newMeshVertices.push_back(vtx);
+  }
+
+  mesh_vertices.clear();
+  mesh_vertices = newMeshVertices;
+
+
+  // update the periodic vertex lists
+
+  std::map<MVertex*,MVertex*> newCorrespondingVertices;
+
+  std::map<MVertex*,MVertex*>::iterator cIter = correspondingVertices.begin();
+  for (;cIter!=correspondingVertices.end();++cIter) {
+    
+    MVertex* tgt = cIter->first;
+    MVertex* src = cIter->second;
+    
+    std::map<MVertex*,MVertex*>::const_iterator tIter = old2new.find(tgt);
+    if (tIter!=old2new.end()) tgt = tIter->second;
+    
+    std::map<MVertex*,MVertex*>::const_iterator sIter = old2new.find(src);
+    if (sIter!=old2new.end()) src = sIter->second;
+    
+    newCorrespondingVertices.insert(std::make_pair(tgt,src));
+
+  }
+  
+  correspondingVertices.clear();
+  correspondingVertices = newCorrespondingVertices;
+
+  newCorrespondingVertices.clear();
+
+  std::map<MVertex*,MVertex*>::iterator hIter = correspondingHOPoints.begin();
+  for (;hIter!=correspondingHOPoints.end();++hIter) {
+    
+    MVertex* tgt = hIter->first;
+    MVertex* src = hIter->second;
+    
+    std::map<MVertex*,MVertex*>::const_iterator tIter = old2new.find(tgt);
+    if (tIter!=old2new.end()) tgt = tIter->second;
+    
+    std::map<MVertex*,MVertex*>::const_iterator sIter = old2new.find(src);
+    if (sIter!=old2new.end()) src = sIter->second;
+    
+    newCorrespondingVertices.insert(std::make_pair(tgt,src));
+
+  }
+  
+  correspondingHOPoints.clear();
+  correspondingHOPoints = newCorrespondingVertices;
+
+}
diff --git a/Geo/GEntity.h b/Geo/GEntity.h
index 9698d36544a69df40876d5dd0b0b2e11f665bc35..be2b45eff71188daa8eca3cd5beed2904e0971d7 100644
--- a/Geo/GEntity.h
+++ b/Geo/GEntity.h
@@ -349,10 +349,18 @@ class GEntity {
   GFace   *cast2Face();
   GRegion *cast2Region();
 
-  // periodic data
+  // update all vertex lists, including periodic connections 
+  void updateVertices(const std::map<MVertex*,MVertex*>&);
+  
+  // transformation from master
   std::vector<double> affineTransform;
+  
+  // corresponding principal vertices
   std::map<MVertex*,MVertex*> correspondingVertices;
 
+  // corresponding high order control points
+  std::map<MVertex*,MVertex*> correspondingHOPoints;
+
 };
 
 class GEntityLessThan {
diff --git a/Geo/GFace.cpp b/Geo/GFace.cpp
index 767f939f633aecf457c8a3954c2b3ebac445a67c..42cd6ea03dfdf255472db2f621cbd32ba5bead7a 100644
--- a/Geo/GFace.cpp
+++ b/Geo/GFace.cpp
@@ -1575,27 +1575,26 @@ void GFace::setMeshMaster(GFace* master,const std::vector<double>& tfo)
   for (mvIter=m_vertices.begin();mvIter!=m_vertices.end();++mvIter) {
 
     GVertex* m_vertex = *mvIter;
-
-    double xyzOri[4] = {m_vertex->x(),
-                        m_vertex->y(),
-                        m_vertex->z(),1};
+    
+    SPoint3 xyzOri((*mvIter)->x(),(*mvIter)->y(),(*mvIter)->z());
     SPoint3 xyzTfo(0,0,0);
 
-    for (size_t i=0,ij=0;i<3;i++) {
-      for (size_t j=0;j<4;j++,ij++) {
-        xyzTfo[i] += tfo[ij] * xyzOri[j];
-      }
+    int idx = 0;
+    for (int i=0;i<3;i++) {
+      for (int j=0;j<3;j++) xyzTfo[i] += xyzOri[j] * tfo[idx++];
+      xyzTfo[i] += tfo[idx++];
     }
-
+    
     GVertex* l_vertex = NULL;
 
     std::set<GVertex*>::iterator lvIter = l_vertices.begin();
     for (;lvIter!=l_vertices.end();++lvIter) {
 
       SPoint3 xyz((*lvIter)->x(),(*lvIter)->y(),(*lvIter)->z());
-      SVector3 dist = xyz - xyzTfo;
+      SVector3 distTfo = xyz - xyzTfo;
+      SVector3 distOri = xyz - xyzOri;
 
-      if (dist.norm() < CTX::instance()->geom.tolerance) {
+      if (distTfo.norm() < CTX::instance()->geom.tolerance * distOri.norm()) {
         l_vertex = *lvIter;
         break;
       }
diff --git a/Geo/GModel.cpp b/Geo/GModel.cpp
index be303a93e66146bdde2c3c55592e8e8f8265a41e..22b30b729c4bda5d2c2b0cfe8e6c889b7dcaa648 100644
--- a/Geo/GModel.cpp
+++ b/Geo/GModel.cpp
@@ -1975,6 +1975,174 @@ static int connectedSurfaceBoundaries(std::set<MEdge, Less_Edge> &edges,
   return boundaries.size();
 }
 
+void GModel::alignPeriodicBoundaries() 
+{
+  Msg::Info("Aligning periodic boundaries");
+
+  // realigning edges 
+
+  for (eiter it = firstEdge();it!=lastEdge();++it) {
+    
+    GEdge* tgt = *it;
+    GEdge* src = dynamic_cast<GEdge*>(tgt->meshMaster());
+    
+    if (src != NULL && src != tgt) {
+      
+      Msg::Info("Aligning periodic edge connection %d-%d",
+                tgt->tag(),src->tag());
+
+      // compose a search list on master edge 
+
+      std::map<MEdge,MLine*,Less_Edge> srcLines;
+      for (unsigned int i=0;i<src->getNumMeshElements();i++)  {
+        MLine* srcLine = dynamic_cast<MLine*>(src->getMeshElement(i));
+        if (!srcLine) Msg::Error("Master element %d is not an edge ",
+                                 src->getMeshElement(i)->getNum());
+        srcLines[MEdge(srcLine->getVertex(0),
+                       srcLine->getVertex(1))] = srcLine;
+      }
+      
+      // run through slave edge elements 
+      // - check whether we find a counterpart (if not, flag error)
+      // - check orientation and reorient if necessary
+
+      for (unsigned int i = 0; i < tgt->getNumMeshElements(); ++i) {
+        
+        MLine* tgtLine = dynamic_cast<MLine*> (tgt->getMeshElement(i));
+        
+        if (!tgtLine) Msg::Error("Slave element %d is not an edge ",
+                            tgt->getMeshElement(i)->getNum());
+        
+        
+        MVertex* tgtVtcs[2];
+        for (int iVtx=0;iVtx<2;iVtx++) {
+          MVertex* tgtVtx = tgtLine->getVertex(iVtx);
+          std::map<MVertex*,MVertex*>& v2v = tgtVtx->onWhat()->correspondingVertices;
+          std::map<MVertex*,MVertex*>::iterator srcIter = v2v.find(tgtVtx);  
+          if (srcIter == v2v.end()) {
+            Msg::Error("Cannot find periodic counterpart of vertex %d"
+                       " of edge %d on edge %d",tgtVtx->getNum(),
+                       tgt->tag(),src->tag());
+          }
+          else tgtVtcs[iVtx] = srcIter->second;
+        }
+        
+        MEdge tgtEdge(tgtVtcs[0],tgtVtcs[1]);
+
+        std::map<MEdge,MLine*,Less_Edge>::iterator sIter = srcLines.find(tgtEdge);
+
+        if (sIter == srcLines.end()) {
+          Msg::Error("Can't find periodic counterpart of edge %d-%d on edge %d"
+                     ", connected to edge %d-%d on %d",
+                     tgtLine->getVertex(0)->getNum(),
+                     tgtLine->getVertex(1)->getNum(),
+                     tgt->tag(),
+                     tgtVtcs[0]->getNum(),
+                     tgtVtcs[1]->getNum(),
+                     src->tag());
+        }
+        else {
+          MLine* srcLine = sIter->second;
+          MEdge  srcEdge(srcLine->getVertex(0),
+                         srcLine->getVertex(1)); 
+          if (tgtEdge.computeCorrespondence(srcEdge)==-1) tgtLine->reverse();
+        }
+      }
+    }
+  }
+
+  // run through all model faces 
+  
+  for(GModel::fiter it = firstFace(); it != lastFace(); ++it) {
+    
+    GFace *tgt = *it;
+    GFace *src = dynamic_cast<GFace*>(tgt->meshMaster());
+    if (src != NULL && src != tgt) {
+      
+      Msg::Info("Aligning periodic face connection %d - %d",tgt->tag(),src->tag());
+      
+      std::map<MFace,MElement*,Less_Face> srcElmts;
+    
+      for (unsigned int i=0;i<src->getNumMeshElements();++i) {
+        MElement* srcElmt = src->getMeshElement(i);
+        int nbVtcs = 0;
+        if (dynamic_cast<MTriangle*>   (srcElmt)) nbVtcs = 3;
+        if (dynamic_cast<MQuadrangle*> (srcElmt)) nbVtcs = 4;
+        std::vector<MVertex*> vtcs;
+        for (int iVtx=0;iVtx<nbVtcs;iVtx++) {
+          vtcs.push_back(srcElmt->getVertex(iVtx));
+        }
+        srcElmts[MFace(vtcs)] = srcElmt;
+      }
+    
+      for (unsigned int i=0;i<tgt->getNumMeshElements();++i) {
+      
+        MElement*    tgtElmt = tgt->getMeshElement(i);
+        MTriangle*   tgtTri = dynamic_cast<MTriangle*>(tgtElmt);
+        MQuadrangle* tgtQua = dynamic_cast<MQuadrangle*>(tgtElmt);
+        
+        int nbVtcs = 0;
+        if (tgtTri) nbVtcs = 3;
+        if (tgtQua) nbVtcs = 4;
+        
+        std::vector<MVertex*> vtcs;
+        for (int iVtx=0;iVtx<nbVtcs;iVtx++) {
+          MVertex* vtx = tgtElmt->getVertex(iVtx);
+          GEntity* ge = vtx->onWhat();
+          if (ge->meshMaster() == ge) throw;
+          std::map<MVertex*,MVertex*>& v2v = ge->correspondingVertices;
+          std::map<MVertex*,MVertex*>::iterator vIter = v2v.find(vtx);
+          if (vIter==v2v.end()) {
+            Msg::Error("Could not find copy of %d in %d",
+                       vtx->getNum(),src->tag(),tgt->tag());
+          }
+          vtcs.push_back(vIter->second);
+        }
+        
+        MFace tgtFace(vtcs);
+
+        std::map<MFace,MElement*>::iterator mIter = srcElmts.find(tgtFace);
+        if (mIter == srcElmts.end()) {
+          std::ostringstream faceDef;
+          for (int iVtx=0;iVtx<nbVtcs;iVtx++) {
+            faceDef << vtcs[iVtx]->getNum() << " ";
+          }
+          Msg::Error("Cannot find periodic counterpart of face %s in face %d "
+                     "connected to %d",faceDef.str().c_str(),
+                     tgt->tag(),src->tag());
+        
+        }
+        else {
+          
+          const MFace& srcFace = mIter->first;
+          MElement* srcElmt = mIter->second;
+          std::vector<MVertex*> srcVtcs;
+
+          if (tgtTri && !dynamic_cast<MTriangle*>(srcElmt)) throw;
+          if (tgtQua && !dynamic_cast<MQuadrangle*>(srcElmt)) throw;
+          
+          int rotation = 0;
+          bool swap = false;
+
+          if (!tgtFace.computeCorrespondence(srcFace,rotation,swap)) {
+            Msg::Error("Non-corresponding face %d-%d-%d (slave) %d-%d-%d (master)",
+                       tgtElmt->getVertex(0)->getNum(),
+                       tgtElmt->getVertex(1)->getNum(),
+                       tgtElmt->getVertex(2)->getNum(),
+                       srcElmt->getVertex(0)->getNum(),
+                       srcElmt->getVertex(1)->getNum(),
+                       srcElmt->getVertex(2)->getNum());
+          }
+          
+          if (tgtTri) tgtTri->reorient(rotation,swap);
+          if (tgtQua) tgtQua->reorient(rotation,swap);
+        }
+      }
+    }
+  }
+  Msg::Info("Done aligning periodic boundaries");
+}
+
 void GModel::makeDiscreteRegionsSimplyConnected()
 {
   Msg::Debug("Making discrete regions simply connected...");
@@ -2444,16 +2612,19 @@ void GModel::createTopologyFromFaces(std::vector<discreteFace*> &discFaces, int
   Msg::Debug("Creating topology for %d edges...", discEdges.size());
 
   // for each discreteEdge, create topology
-  std::map<GFace*, std::map<MVertex*, MVertex*, std::less<MVertex*> > > face2Vert;
-  std::map<GRegion*, std::map<MVertex*, MVertex*, std::less<MVertex*> > > region2Vert;
-  face2Vert.clear();
-  region2Vert.clear();
+  //KH std::map<GFace*, std::map<MVertex*, MVertex*, std::less<MVertex*> > > face2Vert;
+  //KH std::map<GRegion*, std::map<MVertex*, MVertex*, std::less<MVertex*> > > region2Vert;
+  //KH face2Vert.clear();
+  //KH region2Vert.clear();
+
+  std::map<MVertex*,MVertex*> old2new;
   for (std::vector<discreteEdge*>::iterator it = discEdges.begin();
        it != discEdges.end(); it++){
     (*it)->createTopo();
-    (*it)->parametrize(face2Vert, region2Vert);
+    //KH (*it)->parametrize(face2Vert, region2Vert,old2new);
+    (*it)->parametrize(old2new);
   }
-
+  
   // fill edgeLoops of Faces or correct sign of l_edges
   // for (std::vector<discreteFace*>::iterator itF = discFaces.begin();
   //       itF != discFaces.end(); itF++){
@@ -2467,10 +2638,17 @@ void GModel::createTopologyFromFaces(std::vector<discreteFace*> &discFaces, int
   // we need to recreate all mesh elements because some mesh vertices
   // might have been changed during the parametrization process
   // (MVertices became MEdgeVertices)
-  for (std::map<GFace*, std::map<MVertex*, MVertex*, std::less<MVertex*> > >::iterator
-         iFace = face2Vert.begin(); iFace != face2Vert.end(); iFace++){
-    std::map<MVertex*, MVertex*, std::less<MVertex*> > old2new = iFace->second;
-    GFace *gf = iFace->first;
+
+  //KH for (std::map<GFace*, std::map<MVertex*, MVertex*, std::less<MVertex*> > >::iterator
+  //KH        iFace = face2Vert.begin(); iFace != face2Vert.end(); iFace++){
+  //KH   std::map<MVertex*, MVertex*, std::less<MVertex*> > old2new = iFace->second;
+  //KH    GFace *gf = iFace->first;
+  
+  std::set<GFace*,GEntityLessThan>::iterator fIter = faces.begin();
+  for (;fIter!=faces.end();++fIter) {
+
+    GFace* gf = *fIter;
+    
     std::vector<MTriangle*> newTriangles;
     std::vector<MQuadrangle*> newQuadrangles;
     for (unsigned int i = 0; i < gf->getNumMeshElements(); ++i){
@@ -2478,10 +2656,10 @@ void GModel::createTopologyFromFaces(std::vector<discreteFace*> &discFaces, int
       std::vector<MVertex *> v;
       e->getVertices(v);
       for (unsigned int j = 0; j < v.size(); j++){
-        std::map<MVertex*, MVertex*, std::less<MVertex*> >::iterator
-          itmap = old2new.find(v[j]);
-        if (itmap != old2new.end())
-          v[j] = itmap->second;
+        // std::map<MVertex*, MVertex*, std::less<MVertex*> >::iterator
+        //   itmap = old2new.find(v[j]);
+        std::map<MVertex*,MVertex*>::iterator itmap = old2new.find(v[j]);
+        if (itmap != old2new.end()) v[j] = itmap->second;
       }
       MElementFactory factory;
       MElement *e2 = factory.create(e->getTypeForMSH(), v, e->getNum(),
@@ -2498,10 +2676,15 @@ void GModel::createTopologyFromFaces(std::vector<discreteFace*> &discFaces, int
     gf->quadrangles = newQuadrangles;
   }
 
-  for (std::map<GRegion*, std::map<MVertex*, MVertex*, std::less<MVertex*> > >::iterator
-         iRegion = region2Vert.begin(); iRegion != region2Vert.end(); iRegion++){
-    std::map<MVertex*, MVertex*, std::less<MVertex*> > old2new = iRegion->second;
-    GRegion *gr = iRegion->first;
+  // for (std::map<GRegion*, std::map<MVertex*, MVertex*, std::less<MVertex*> > >::iterator
+  //        iRegion = region2Vert.begin(); iRegion != region2Vert.end(); iRegion++){
+  //   std::map<MVertex*, MVertex*, std::less<MVertex*> > old2new = iRegion->second;
+  //   GRegion *gr = iRegion->first;
+  for (std::set<GRegion*,GEntityLessThan>::iterator rIter = regions.begin();
+       rIter!=regions.end();++rIter) {
+
+    GRegion* gr = *rIter;
+
     std::vector<MTetrahedron*> newTetrahedra;
     std::vector<MHexahedron*> newHexahedra;
     std::vector<MPrism*> newPrisms;
@@ -2512,10 +2695,13 @@ void GModel::createTopologyFromFaces(std::vector<discreteFace*> &discFaces, int
       std::vector<MVertex *> v;
       e->getVertices(v);
       for (unsigned int j = 0; j < v.size(); j++){
-        std::map<MVertex*, MVertex*, std::less<MVertex*> >::iterator
-          itmap = old2new.find(v[j]);
-        if (itmap != old2new.end())
-          v[j] = itmap->second;
+        // std::map<MVertex*, MVertex*, std::less<MVertex*> >::iterator
+        //   itmap = old2new.find(v[j]);
+        // if (itmap != old2new.end())
+        //   v[j] = itmap->second;
+        std::map<MVertex*,MVertex*>::iterator itmap = old2new.find(v[j]);
+        if (itmap != old2new.end()) v[j] = itmap->second;
+
       }
       MElementFactory factory;
       MElement *e2 = factory.create(e->getTypeForMSH(), v, e->getNum(),
@@ -2540,7 +2726,18 @@ void GModel::createTopologyFromFaces(std::vector<discreteFace*> &discFaces, int
     gr->pyramids = newPyramids;
     gr->trihedra = newTrihedra;
   }
-
+  
+  // -- now correct periodicity information 
+
+  std::set<GFace*,GEntityLessThan>::iterator gfIter = faces.begin();
+  for (;gfIter!=faces.end();++gfIter) (*gfIter)->updateVertices(old2new);
+  
+  std::set<GEdge*,GEntityLessThan>::iterator geIter = edges.begin();
+  for (;geIter!=edges.end();++geIter) (*geIter)->updateVertices(old2new);
+  
+  std::set<GVertex*,GEntityLessThan>::iterator gvIter = vertices.begin();
+  for (;gvIter!=vertices.end();++gvIter) (*gvIter)->updateVertices(old2new);
+  
   Msg::Debug("Done creating topology for edges");
 }
 
diff --git a/Geo/GModel.h b/Geo/GModel.h
index 253172e5dfecd341b70cef57af499b3480dbea9b..a9d3c9fb8ba53a88dfcd508f47ab9910b3345b67 100644
--- a/Geo/GModel.h
+++ b/Geo/GModel.h
@@ -429,6 +429,9 @@ class GModel
   void createTopologyFromFaces(std::vector<discreteFace*> &pFaces, int ignoreHoles=0);
   void makeDiscreteRegionsSimplyConnected();
   void makeDiscreteFacesSimplyConnected();
+  
+  // align periodic boundaries
+  void alignPeriodicBoundaries();
 
   // a container for smooth normals
   smooth_normals *normals;
diff --git a/Geo/GModelIO_MSH2.cpp b/Geo/GModelIO_MSH2.cpp
index 007f883f80494e2cf88e706feb3160643bd6b321..ece2fd82cc6313fde1e653a03479548f435e7e74 100644
--- a/Geo/GModelIO_MSH2.cpp
+++ b/Geo/GModelIO_MSH2.cpp
@@ -719,6 +719,8 @@ int GModel::_readMSH2(const std::string &name)
 
   fclose(fp);
 
+  alignPeriodicBoundaries();
+
   return postpro ? 2 : 1;
 }
 
diff --git a/Geo/GeomMeshMatcher.cpp b/Geo/GeomMeshMatcher.cpp
index 8d2a52d22b68e7cc4a540001f25836fec84690e1..492f5b669d5842583e442c846bc93dfbb9cba476 100644
--- a/Geo/GeomMeshMatcher.cpp
+++ b/Geo/GeomMeshMatcher.cpp
@@ -27,6 +27,7 @@
 #include "MPoint.h"
 #include "MHexahedron.h"
 #include "MTetrahedron.h"
+#include "OS.h"
 
 GeomMeshMatcher *GeomMeshMatcher::_gmm_instance = 0;
 
@@ -582,9 +583,10 @@ int GeomMeshMatcher::forceTomatch(GModel *geom, GModel *mesh, const double TOL)
 }
 
 template <class GEType>
-static void copy_periodicity (std::vector<Pair<GEType*, GEType*> >& eCor)
+static void copy_periodicity (std::vector<Pair<GEType*, GEType*> >& eCor,
+                              std::map<MVertex*,MVertex*>& mesh_to_geom)
 {
-
+    
   typename std::multimap<GEType*,GEType*> eMap; // (eCor.begin(),eCor.end());
   typename std::vector<Pair<GEType*,GEType*> >::iterator eIter = eCor.begin();
   for (;eIter!=eCor.end();++eIter) {
@@ -608,6 +610,30 @@ static void copy_periodicity (std::vector<Pair<GEType*, GEType*> >& eCor)
       }
       GEType* newSrc = tgtIter->second;
       newTgt->setMeshMaster(newSrc,oldTgt->affineTransform);
+      
+      std::map<MVertex*,MVertex*>& oldV2v = oldTgt->correspondingVertices;
+      std::map<MVertex*,MVertex*>& newV2v = newTgt->correspondingVertices;
+
+      std::map<MVertex*,MVertex*>::iterator vIter = oldV2v.begin();
+      for (;vIter!=oldV2v.end();++vIter) {
+
+        MVertex* oldTgtV = vIter->first;
+        MVertex* oldSrcV = vIter->second;
+        
+        std::map<MVertex*,MVertex*>::iterator newTvIter = mesh_to_geom.find(oldTgtV);
+        std::map<MVertex*,MVertex*>::iterator newSvIter = mesh_to_geom.find(oldSrcV);
+        
+        if (newTvIter == mesh_to_geom.end()) {
+          Msg::Error("Could not find copy of target vertex %d in entity %d of dim",
+                     oldTgtV->getIndex(),oldTgt->tag(),oldTgt->dim());
+        }
+        
+        if (newSvIter == mesh_to_geom.end()) {
+          Msg::Error("Could not find copy of source vertex %d in entity %d of dim",
+                     oldSrcV->getIndex(),oldSrc->tag(),oldSrc->dim());
+        }
+        newV2v[newTvIter->second] = newSvIter->second;
+      }
     }
   }
 }
@@ -653,11 +679,13 @@ static void copy_vertices (GEdge* to, GEdge* from, std::map<MVertex*,MVertex*> &
     double t;
     GPoint gp = to->closestPoint(SPoint3(v_from->x(),v_from->y(),v_from->z()), t );
     MEdgeVertex *v_to = new MEdgeVertex (gp.x(),gp.y(),gp.z(), to, gp.u() );
+    
     to->mesh_vertices.push_back(v_to);
     _mesh_to_geom[v_from] = v_to;
   }
   //  printf("Ending Edge %d %d vertices to match\n",from->tag(),from->mesh_vertices.size());
 }
+
 static void copy_vertices (GFace *geom, GFace *mesh, std::map<MVertex*,MVertex*> &_mesh_to_geom){
   //  printf("Starting Face %d, with %d vertices\n", geom->tag(),  mesh->mesh_vertices.size());
   for (unsigned int i=0;i<mesh->mesh_vertices.size();i++){
@@ -683,8 +711,8 @@ static void copy_vertices (GFace *geom, GFace *mesh, std::map<MVertex*,MVertex*>
 
 template <class ELEMENT>
 static void copy_elements (std::vector<ELEMENT*> &to,
-			   std::vector<ELEMENT*> &from,
-			   std::map<MVertex*,MVertex*> &_mesh_to_geom){
+                           std::vector<ELEMENT*> &from,
+                           std::map<MVertex*,MVertex*> &_mesh_to_geom){
   MElementFactory toto;
   to.clear();
   for (unsigned int i=0;i < from.size();i++){
@@ -693,7 +721,7 @@ static void copy_elements (std::vector<ELEMENT*> &to,
     for(int j=0;j<e->getNumVertices();j++) {
       nodes.push_back(_mesh_to_geom[e->getVertex(j)]);
       if (_mesh_to_geom[e->getVertex(j)] == 0) {
-	printf("Error vertex %i\n", e->getVertex(j)->getNum());
+        printf("Error vertex %i\n", e->getVertex(j)->getNum());
       }
     }
     to.push_back( (ELEMENT*)(toto.create(e->getTypeForMSH(), nodes) ));
@@ -761,6 +789,10 @@ void copy_elements (GModel *geom, GModel *mesh, std::map<MVertex*,MVertex*> &_me
 
 int GeomMeshMatcher::match(GModel *geom, GModel *mesh)
 {
+  
+  Msg::StatusBar(true,"Matching discrete mesh to actual CAD ...");  
+  double t1 = Cpu();
+
   mesh->createTopologyFromMesh();
   GModel::setCurrent(geom);
 
@@ -772,18 +804,20 @@ int GeomMeshMatcher::match(GModel *geom, GModel *mesh)
   std::vector<Pair<GRegion*, GRegion*> > *coresp_r = matchRegions (geom, mesh, coresp_f, ok);
 
   std::map<MVertex*,MVertex*> _mesh_to_geom;
-
-  copy_periodicity(*coresp_v);
-  copy_periodicity(*coresp_e);
-  copy_periodicity(*coresp_f);
   
-  copy_vertices(geom, mesh, _mesh_to_geom,coresp_v,coresp_e,coresp_f,coresp_r);
-  copy_elements(geom, mesh, _mesh_to_geom,coresp_v,coresp_e,coresp_f,coresp_r);
-
+  copy_vertices(geom,mesh,_mesh_to_geom,coresp_v,coresp_e,coresp_f,coresp_r);
+  copy_elements(geom,mesh,_mesh_to_geom,coresp_v,coresp_e,coresp_f,coresp_r);
+  
+  copy_periodicity(*coresp_v,_mesh_to_geom);
+  copy_periodicity(*coresp_e,_mesh_to_geom);
+  copy_periodicity(*coresp_f,_mesh_to_geom);
+  
   delete coresp_v;
   delete coresp_e;
   delete coresp_f;
   delete coresp_r;
 
+  Msg::StatusBar(true,"Done matching discrete mesh to actual CAD",Cpu()-t1);
+
   return 1;
 }
diff --git a/Geo/MEdge.h b/Geo/MEdge.h
index d398080964d412f19b78a166a32c91e2dbc5d34b..afc36776e930efbb5d709dc3288102c619237c0e 100644
--- a/Geo/MEdge.h
+++ b/Geo/MEdge.h
@@ -39,6 +39,13 @@ class MEdge {
   inline MVertex *getSortedVertex(const int i) const { return _v[int(_si[i])]; }
   inline MVertex *getMinVertex() const { return _v[int(_si[0])]; }
   inline MVertex *getMaxVertex() const { return _v[int(_si[1])]; }
+  
+  inline int computeCorrespondence(MEdge& other) {
+    if (other.getVertex(0) == _v[0] && other.getVertex(1) == _v[1]) return 1;
+    else if (other.getVertex(0) == _v[1] && other.getVertex(1) == _v[0]) return -1;
+    return 0;
+  }
+
   SVector3 scaledTangent() const
   {
     return SVector3(_v[1]->x() - _v[0]->x(),
diff --git a/Geo/MFace.cpp b/Geo/MFace.cpp
index 526a5c4df36bcf8df44c1afc0dfc9ed97df99d2b..1cef6e972b48e7b9614f7c0e778c709b442fa9b2 100644
--- a/Geo/MFace.cpp
+++ b/Geo/MFace.cpp
@@ -92,7 +92,9 @@ SVector3 MFace::normal() const
   return SVector3(n[0], n[1], n[2]);
 }
 
-bool MFace::computeCorrespondence(const MFace &other, int &rotation, bool &swap) const
+bool MFace::computeCorrespondence(const MFace &other, 
+                                  int &rotation, 
+                                  bool &swap) const
 {
   rotation = 0;
   swap = false;
diff --git a/Geo/MQuadrangle.cpp b/Geo/MQuadrangle.cpp
index 4216c4d05ec8c137533a9f310f89e47c347bb8c0..9aa42dea1e7ce87597f4ee1eca916bfe7a980910 100644
--- a/Geo/MQuadrangle.cpp
+++ b/Geo/MQuadrangle.cpp
@@ -14,6 +14,8 @@
 #include "qualityMeasures.h"
 #endif
 
+#include <cstring>
+
 #define SQU(a)      ((a)*(a))
 
 int MQuadrangleN::getNumEdgesRep(bool curved)
@@ -325,3 +327,77 @@ double MQuadrangle::getInnerRadius()
                   );
 #endif // HAVE_LAPACK
 }
+
+
+void MQuadrangle::reorient(int rot, bool swap) {
+  MVertex* tmp[4];
+  if (swap) for (int i=0;i<4;i++) tmp[i] = _v[(8-i-rot)%4];
+  else      for (int i=0;i<4;i++) tmp[i] = _v[(4+i-rot)%4];
+  std::memcpy(_v,tmp,4*sizeof(MVertex*));
+}
+
+void MQuadrangle8::reorient(int rot, bool swap) { 
+  
+  if (rot == 0 && !swap) return;
+  
+  MQuadrangle::reorient(rot,swap);
+  MVertex* tmp[4];
+  if (swap) for (int i=0;i<4;i++) tmp[i] = _vs[(8-i-rot)%4];
+  else      for (int i=0;i<4;i++) tmp[i] = _vs[(4+i-rot)%4];
+  std::memcpy(_vs,tmp,4*sizeof(MVertex*));
+}
+
+void MQuadrangle9::reorient(int rot, bool swap) { 
+
+  if (rot == 0 && !swap) return;
+  
+  MQuadrangle::reorient(rot,swap);
+  MVertex* tmp[4];
+  if (swap) for (int i=0;i<4;i++) tmp[i] = _vs[(9-i-rot)%4]; // edge swapped
+  else      for (int i=0;i<4;i++) tmp[i] = _vs[(4+i-rot)%4];
+  std::memcpy(_vs,tmp,4*sizeof(MVertex*));
+}
+
+void MQuadrangleN::reorient(int rot, bool swap) {
+
+  if (rot == 0 && !swap) return;
+
+  MQuadrangle::reorient(rot,swap);
+  int order  = getPolynomialOrder();
+  int nbEdgePts = order - 1;
+  unsigned int idx = 0;
+
+  std::vector<MVertex*> tmp;
+
+  if (swap) {
+    for (int iEdge=0;iEdge<4;iEdge++) {
+      int edgeIdx = ((9-iEdge-rot)%4)*nbEdgePts;
+      for (int i=nbEdgePts-1;i>=0;i--) tmp.push_back(_vs[edgeIdx+i]);
+    }
+  }
+  else {
+    for (int iEdge=0;iEdge<4;iEdge++) {
+      int edgeIdx = ((4+iEdge-rot)%4)*nbEdgePts;
+      for (int i=0;i<nbEdgePts;i++)    tmp.push_back(_vs[edgeIdx+i]);
+    }
+  }
+  
+  idx += 4*nbEdgePts;
+
+  if (_vs.size() >= idx) {
+    nbEdgePts = order - 3;
+    if (order > 2) {
+      if (swap) for (int i=0;i<4;i++) tmp.push_back(_vs[idx + (8-i-rot)%4]);
+      else      for (int i=0;i<4;i++) tmp.push_back(_vs[idx + (4+i-rot)%4]);
+      idx += 4;
+      if (order > 3) {
+        if (swap) for (int i=0;i<4;i++) tmp.push_back(_vs[idx + (9-i-rot)%4]);
+        else      for (int i=0;i<4;i++) tmp.push_back(_vs[idx + (4+i-rot)%4]);
+        idx += 4;
+        if (order > 4) Msg::Error("Reorientation of quad not supported above order 4");
+      } 
+    }
+    tmp.push_back(_vs[idx]);
+  }
+  _vs = tmp;
+}
diff --git a/Geo/MQuadrangle.h b/Geo/MQuadrangle.h
index d9abc86b309b549e1d31c114aaf783894818c400..1b8839ed571f566acd691edf63d347b260ee10bb 100644
--- a/Geo/MQuadrangle.h
+++ b/Geo/MQuadrangle.h
@@ -137,6 +137,11 @@ class MQuadrangle : public MElement {
   {
     MVertex *tmp = _v[1]; _v[1] = _v[3]; _v[3] = tmp;
   }
+  // reorient the quadrangle to conform with other face
+  // orientation computed with MFace based on this face with respect to other 
+  // in computeCorrespondence
+  virtual void reorient(int rotation, bool swap);
+  
   virtual bool isInside(double u, double v, double w) const
   {
     double tol = _isInsideTolerance;
@@ -242,6 +247,12 @@ class MQuadrangle8 : public MQuadrangle {
     tmp = _vs[0]; _vs[0] = _vs[3]; _vs[3] = tmp;
     tmp = _vs[1]; _vs[1] = _vs[2]; _vs[2] = tmp;
   }
+
+  // reorient the quadrangle to conform with other face
+  // orientation computed with MFace based on this face with respect to other 
+  // in computeCorrespondence
+  virtual void reorient(int rotation, bool swap);
+  
   virtual void getNode(int num, double &u, double &v, double &w) const
   {
     num < 4 ? MQuadrangle::getNode(num, u, v, w) : MElement::getNode(num, u, v, w);
@@ -324,6 +335,12 @@ class MQuadrangle9 : public MQuadrangle {
     tmp = _vs[0]; _vs[0] = _vs[3]; _vs[3] = tmp;
     tmp = _vs[1]; _vs[1] = _vs[2]; _vs[2] = tmp;
   }
+
+  // reorient the quadrangle to conform with other face
+  // orientation computed with MFace based on this face with respect to other 
+  // in computeCorrespondence
+  virtual void reorient(int rotation, bool swap);
+  
   virtual void getNode(int num, double &u, double &v, double &w) const
   {
     num < 4 ? MQuadrangle::getNode(num, u, v, w) : MElement::getNode(num, u, v, w);
@@ -442,6 +459,12 @@ class MQuadrangleN : public MQuadrangle {
     inv.insert(inv.begin(), _vs.rbegin(), _vs.rend());
     _vs = inv;
   }
+  
+  // reorient the quadrangle to conform with other face
+  // orientation computed with MFace based on this face with respect to other 
+  // in computeCorrespondence
+  virtual void reorient(int rotation, bool swap);
+  
   virtual void getNode(int num, double &u, double &v, double &w) const
   {
     num < 4 ? MQuadrangle::getNode(num, u, v, w) : MElement::getNode(num, u, v, w);
diff --git a/Geo/MTriangle.cpp b/Geo/MTriangle.cpp
index 53b3f01729b7fc0da34b0062b49b5db7b20d8ccf..ec79b528582b09179f6b0b307255d15feefa2ff6 100644
--- a/Geo/MTriangle.cpp
+++ b/Geo/MTriangle.cpp
@@ -13,6 +13,8 @@
 #include "qualityMeasures.h"
 #endif
 
+#include <cstring>
+
 #define SQU(a)      ((a)*(a))
 
 SPoint3 MTriangle::circumcenter()
@@ -260,3 +262,68 @@ void MTriangle::getIntegrationPoints(int pOrder, int *npts, IntPt **pts)
   *npts = getNGQTPts(pOrder);
   *pts = getGQTPts(pOrder);
 }
+
+void MTriangle::reorient(int rot,bool swap) {
+  
+  if (rot == 0 && !swap) return;
+  
+  MVertex* tmp[3];
+  std::memcpy(tmp,_v,3*sizeof(MVertex*));
+  if (swap) for (int i=0;i<3;i++) _v[i] = tmp[(6-i-rot)%3];
+  else      for (int i=0;i<3;i++) _v[i] = tmp[(3+i-rot)%3];
+}
+
+void MTriangle6::reorient(int rot, bool swap) {
+
+  if (rot == 0 && !swap) return;
+
+  MTriangle::reorient(rot,swap);
+  MVertex* tmp[3];
+  std::memcpy(tmp,_vs,3*sizeof(MVertex*));
+  if (swap) for (int i=0;i<3;i++) _vs[i] = tmp[(7-i-rot)%3];
+  else      for (int i=0;i<3;i++) _vs[i] = tmp[(3+i-rot)%3];
+}
+
+void MTriangleN::reorient(int rot, bool swap) {
+  
+  if (rot == 0 && !swap) return;
+
+  MTriangle::reorient(rot,swap);
+  
+  std::vector<MVertex*> tmp;
+  int order  = getPolynomialOrder();
+  int nbEdge =  order - 1;
+  unsigned int idx = 0; 
+  
+  
+  if (swap) {
+    for (int iEdge=0;iEdge<3;iEdge++) {
+      int edgeIdx = ((7-iEdge-rot)%3)*nbEdge;
+      for (int i=nbEdge-1;i>=0;i--) tmp.push_back(_vs[edgeIdx + i]);
+    }
+  }
+  else {
+    for (int iEdge=0;iEdge<3;iEdge++) {
+      int edgeIdx = ((3+iEdge-rot)%3)*nbEdge;
+      for (int i=0;i<nbEdge;i++) tmp.push_back(_vs[edgeIdx + i]);
+    }
+  }
+
+  idx += 3*nbEdge;
+  
+  if (_vs.size() > idx ) {
+    if (order == 3) tmp.push_back(_vs[idx]);
+    if (order == 4) {
+      if (swap) for(int i=0;i<3;i++) tmp.push_back(_vs[idx+(6-rot-i)%3]);
+      else      for(int i=0;i<3;i++) tmp.push_back(_vs[idx+(3+i-rot)%3]);
+    }
+    if (order >=5) 
+      Msg::Error("Reorientation of a triangle not supported above order 4");
+  }
+  _vs = tmp;
+}
+
+
+
+  
+
diff --git a/Geo/MTriangle.h b/Geo/MTriangle.h
index 08c72ac0ac7869af0122d29fcc994e2627a8e47f..fe46e28e0a433eb1031035128b4370bdacc33f51 100644
--- a/Geo/MTriangle.h
+++ b/Geo/MTriangle.h
@@ -124,6 +124,12 @@ class MTriangle : public MElement {
   {
     MVertex *tmp = _v[1]; _v[1] = _v[2]; _v[2] = tmp;
   }
+  
+  // reorient the triangle to conform with other face
+  // orientation computed with MFace based on this face with respect to other 
+  // in computeCorrespondence
+  virtual void reorient(int rotation, bool swap);
+  
   virtual void getNode(int num, double &u, double &v, double &w) const
   {
     w = 0.;
@@ -235,6 +241,10 @@ class MTriangle6 : public MTriangle {
   {
     num < 3 ? MTriangle::getNode(num, u, v, w) : MElement::getNode(num, u, v, w);
   }
+  // reorient the triangle to conform with other face
+  // orientation computed with MFace based on this face with respect to other 
+  // in computeCorrespondence
+  virtual void reorient(int rotation, bool swap);
 };
 
 /*
@@ -342,6 +352,10 @@ class MTriangleN : public MTriangle {
   {
     num < 3 ? MTriangle::getNode(num, u, v, w) : MElement::getNode(num, u, v, w);
   }
+  // reorient the triangle to conform with other face
+  // orientation computed with MFace based on this face with respect to other 
+  // in computeCorrespondence
+  virtual void reorient(int rotation, bool swap);
 };
 
 template <class T>
diff --git a/Geo/discreteEdge.cpp b/Geo/discreteEdge.cpp
index 1b8fc0ec292bbace3ba9a613f2d87bb913c408d1..a742e6752893cb6ae4ce4efbf01b0abe2bcf76f1 100644
--- a/Geo/discreteEdge.cpp
+++ b/Geo/discreteEdge.cpp
@@ -316,40 +316,85 @@ void discreteEdge::parametrize(std::map<GFace*, std::map<MVertex*, MVertex*,
    for(std::list<GFace*>::iterator iFace = l_faces.begin();
        iFace != l_faces.end(); ++iFace){
 
-     // for each face find correspondane face2Vertex
-     std::map<GFace*, std::map<MVertex*, MVertex*,
-       std::less<MVertex*> > >::iterator itmap = face2Vert.find(*iFace);
-     if (itmap == face2Vert.end()) {
-       face2Vert.insert(std::make_pair(*iFace, old2new));
-     }
-     else{
-       std::map<MVertex*, MVertex*, std::less<MVertex*> > mapVert = itmap->second;
-       for (std::map<MVertex*, MVertex*, std::less<MVertex*> >::iterator it =
-              old2new.begin(); it != old2new.end(); it++)
-         mapVert.insert(*it);
-       itmap->second = mapVert;
-     }
+
+     face2Vert[*iFace].insert(old2new.begin(),old2new.end());
+
+     // // for each face find correspondane face2Vertex
+     // std::map<GFace*, std::map<MVertex*, MVertex*,
+     //   std::less<MVertex*> > >::iterator itmap = face2Vert.find(*iFace);
+     // if (itmap == face2Vert.end()) {
+     //   face2Vert.insert(std::make_pair(*iFace, old2new));
+     // }
+     // else{
+     //   std::map<MVertex*, MVertex*, std::less<MVertex*> > mapVert = itmap->second;
+     //   for (std::map<MVertex*, MVertex*, std::less<MVertex*> >::iterator it =
+     //          old2new.begin(); it != old2new.end(); it++)
+     //     mapVert.insert(*it);
+     //   itmap->second = mapVert;
+     // }
 
      // do the same for regions
      for ( int j = 0; j < (*iFace)->numRegions(); j++){
        GRegion *r = (*iFace)->getRegion(j);
-       std::map<GRegion*,  std::map<MVertex*, MVertex*,
-         std::less<MVertex*> > >::iterator itmap = region2Vert.find(r);
-       if (itmap == region2Vert.end()) {
-	 region2Vert.insert(std::make_pair(r, old2new));
-       }
-       else{
-	 std::map<MVertex*, MVertex*, std::less<MVertex*> > mapVert = itmap->second;
-	 for (std::map<MVertex*, MVertex*, std::less<MVertex*> >::iterator it =
-                old2new.begin(); it != old2new.end(); it++)
-	   mapVert.insert(*it);
-	 itmap->second = mapVert;
-       }
+       region2Vert[r].insert(old2new.begin(),old2new.end());
+
+       //     std::map<GRegion*,  std::map<MVertex*, MVertex*,
+       //       std::less<MVertex*> > >::iterator itmap = region2Vert.find(r);
+       //     if (itmap == region2Vert.end()) {
+       // region2Vert.insert(std::make_pair(r, old2new));
+       //     }
+       //     else{
+       // std::map<MVertex*, MVertex*, std::less<MVertex*> > mapVert = itmap->second;
+       // for (std::map<MVertex*, MVertex*, std::less<MVertex*> >::iterator it =
+       //              old2new.begin(); it != old2new.end(); it++)
+       //   mapVert.insert(*it);
+       // itmap->second = mapVert;
+       //     }
      }
-
    }
 }
 
+
+
+void discreteEdge::parametrize(std::map<MVertex*, MVertex*>& old2new)
+{
+  if (_pars.empty()){
+    for (unsigned int i = 0; i < lines.size() + 1; i++){
+      _pars.push_back(i);
+    }
+  }
+  
+  std::vector<MVertex* > newVertices;
+  std::vector<MLine*> newLines;
+
+  MVertex *vL = getBeginVertex()->mesh_vertices[0];
+  int i = 0;
+  for(i = 0; i < (int)lines.size() - 1; i++){
+    MVertex *vR;
+    if (_orientation[i] == 1 ) vR = lines[i]->getVertex(1);
+    else vR = lines[i]->getVertex(0);
+    int param = i+1;
+    MVertex *vNEW = new MEdgeVertex(vR->x(),vR->y(),vR->z(), this,
+                                    param, -1., vR->getNum());
+    old2new.insert(std::make_pair(vR,vNEW));
+    newVertices.push_back(vNEW);
+    newLines.push_back(new MLine(vL, vNEW));
+    _orientation[i] = 1;
+    vL = vNEW;
+  }
+  MVertex *vR = getEndVertex()->mesh_vertices[0];
+  newLines.push_back(new MLine(vL, vR));
+  _orientation[i] = 1;
+
+  deleteVertexArrays();
+  model()->destroyMeshCaches();
+
+  mesh_vertices = newVertices;
+  lines.clear();
+  lines = newLines;  
+}
+
+
 void discreteEdge::computeNormals () const
 {
   _normals.clear();
diff --git a/Geo/discreteEdge.h b/Geo/discreteEdge.h
index a44c18559046c0383cb6700518bcc5e82a551f8b..a73a6bcf1c08ba444962a39fcd13a0a529af1d88 100644
--- a/Geo/discreteEdge.h
+++ b/Geo/discreteEdge.h
@@ -34,9 +34,11 @@ class discreteEdge : public GEdge {
   bool getLocalParameter(const double &t, int &iEdge, double &tLoc) const;
   void interpolateInGeometry (MVertex *v, MVertex **v1, MVertex **v2, double &xi) const; 
   void parametrize(std::map<GFace*, std::map<MVertex*, MVertex*,
-		   std::less<MVertex*> > > &face2Verts,
-		   std::map<GRegion*, std::map<MVertex*, MVertex*,
-		   std::less<MVertex*> > > &region2Vert);
+                   std::less<MVertex*> > > &face2Verts,
+                   std::map<GRegion*, std::map<MVertex*, MVertex*,
+                   std::less<MVertex*> > > &region2Vert);
+  void parametrize(std::map<MVertex*,MVertex*>& old2New);
+  
   void orderMLines();
   void setBoundVertices();
   void createTopo();
diff --git a/Mesh/CMakeLists.txt b/Mesh/CMakeLists.txt
index 7088580950a32ab51a7b2e3eb9319fe5c0249e6c..7cf93c40fc481880ba36b735f5e78906e95d157d 100644
--- a/Mesh/CMakeLists.txt
+++ b/Mesh/CMakeLists.txt
@@ -9,7 +9,6 @@ set(SRC
    delaunay_refinement.cpp
    Generator.cpp
     meshGEdge.cpp 
-   meshGEntity.cpp
       meshGEdgeExtruded.cpp
     meshGFace.cpp 
     meshGFaceElliptic.cpp
diff --git a/Mesh/HighOrder.cpp b/Mesh/HighOrder.cpp
index 5cfe4040f354fe9976bc694e510b53994087a9f6..bc794203279c1fcee2dc588cfe6cb620a85a0bef 100644
--- a/Mesh/HighOrder.cpp
+++ b/Mesh/HighOrder.cpp
@@ -23,6 +23,8 @@
 #include "BasisFactory.h"
 #include "MVertexRTree.h"
 
+#include <sstream>
+
 // --------- Functions that help optimizing placement of points on geometry -----------
 
 // The aim here is to build a polynomial representation that consist
@@ -1292,7 +1294,8 @@ static void setFirstOrder(GEntity *e, std::vector<T*> &elements, bool onlyVisibl
 }
 
 static void updateHighOrderVertices(GEntity *e,
-                                    const std::vector<MVertex*> &newHOVert, bool onlyVisible)
+                                    const std::vector<MVertex*> &newHOVert, 
+                                    bool onlyVisible)
 {
   if (onlyVisible && !e->getVisibility())return;
   std::vector<MVertex*> v1;
@@ -1309,70 +1312,132 @@ static void updateHighOrderVertices(GEntity *e,
 
 static void updatePeriodicEdgesAndFaces(GModel *m)
 {
+
+  // Edges 
+  
   for(GModel::eiter it = m->firstEdge(); it != m->lastEdge(); ++it) {
-    GEdge *slave = *it;
-    GEdge *master = dynamic_cast<GEdge*>(slave->meshMaster());
-    if (master == slave) continue;
-
-    std::map<MVertex*,MVertex*> &v2v = slave->correspondingVertices;
-    v2v.clear();
-
-    MVertexRTree rtree = MVertexRTree(1e-8 * CTX::instance()->lc);
-    for (unsigned int i = 0; i < slave->getNumMeshVertices(); ++i)
-      rtree.insert(slave->getMeshVertex(i));
-
-    std::vector<double> tfo = slave->affineTransform;
-
-    if(tfo.size() >= 16){
-      for (unsigned int i = 0; i < master->getNumMeshVertices(); ++i) {
-        MVertex *vs = master->getMeshVertex(i);
-        double ps[4] = {vs->x(), vs->y(), vs->z(), 1.};
-        double res[4] = {0., 0., 0., 0.};
-        int idx = 0;
-        for(int i = 0; i < 4; i++)
-          for(int j = 0; j < 4; j++)
-            res[i] +=  tfo[idx++] * ps[j];
-        SPoint3 p3 (res[0], res[1], res[2]);
-        double u = slave->parFromPoint(p3);
-        GPoint gp = slave->point(u);
-        MVertex *vt = rtree.find(gp.x(), gp.y(), gp.z());
-        if (!vt) Msg::Error("Couldn't find a vertex for updating periodicity");
-        else v2v[vt] = vs;
+    
+    GEdge *tgt = *it;
+    GEdge *src = dynamic_cast<GEdge*>(tgt->meshMaster());
+    
+    if (src != NULL && src != tgt) {
+      
+      std::map<MVertex*,MVertex*> &p2p = tgt->correspondingHOPoints;
+      p2p.clear();
+      
+      Msg::Info("Constructing high order periodicity for edge connection %d - %d",
+                tgt->tag(),src->tag());
+      
+      std::map<MEdge,MLine*,Less_Edge> srcEdges;
+      for (unsigned int i=0;i<src->getNumMeshElements();i++)  {
+        MLine* srcLine = dynamic_cast<MLine*>(src->getMeshElement(i));
+        if (!srcLine) Msg::Error("Master element %d is not an edge ",
+                                 src->getMeshElement(i)->getNum());
+        srcEdges[MEdge(srcLine->getVertex(0),
+                       srcLine->getVertex(1))] = srcLine;
+      }
+      
+      for (unsigned int i = 0; i < tgt->getNumMeshElements(); ++i) {
+        
+        MLine* tgtLine = dynamic_cast<MLine*> (tgt->getMeshElement(i));
+        MVertex* vtcs[2];
+        
+        if (!tgtLine) Msg::Error("Slave element %d is not an edge ",
+                            tgt->getMeshElement(i)->getNum());
+        
+        for (int iVtx=0;iVtx<2;iVtx++) {
+          MVertex* vtx = tgtLine->getVertex(iVtx);
+          std::map<MVertex*,MVertex*>& v2v = 
+            vtx->onWhat()->correspondingVertices;
+          std::map<MVertex*,MVertex*>::iterator tIter = v2v.find(vtx);  
+          if (tIter == v2v.end()) {
+            Msg::Error("Cannot find periodic counterpart of vertex %d"
+                       " of edge %d on edge %d",
+                       vtx->getNum(),tgt->tag(),src->tag());
+          }
+          else vtcs[iVtx] = tIter->second;
+        }
+        
+        std::map<MEdge,MLine*,Less_Edge>::iterator srcIter = srcEdges.find(MEdge(vtcs[0],vtcs[1]));
+        if (srcIter==srcEdges.end()) {
+          Msg::Error("Can't find periodic counterpart of edge %d-%d on edge %d"
+                     ", connected to edge %d-%d on %d",
+                     tgtLine->getVertex(0)->getNum(),
+                     tgtLine->getVertex(1)->getNum(),
+                     tgt->tag(),
+                     vtcs[0]->getNum(),
+                     vtcs[1]->getNum(),
+                     src->tag());
+        }
+        else {
+          MLine* srcLine = srcIter->second;
+          if (tgtLine->getNumVertices() != srcLine->getNumVertices()) throw;
+          for (int i=2;i<tgtLine->getNumVertices();i++) p2p[tgtLine->getVertex(i)] = srcLine->getVertex(i);
+        }
       }
     }
   }
 
   for(GModel::fiter it = m->firstFace(); it != m->lastFace(); ++it) {
-    GFace *slave = *it;
-    GFace *master = dynamic_cast<GFace*>(slave->meshMaster());
-    if (master == slave) continue;
-
-    std::map<MVertex*,MVertex*> &v2v = slave->correspondingVertices;
-    v2v.clear();
-
-    MVertexRTree rtree = MVertexRTree(1e-8 * CTX::instance()->lc);
-    for (unsigned int i = 0; i < slave->getNumMeshVertices(); ++i)
-      rtree.insert(slave->getMeshVertex(i));
-
-    std::vector<double> tfo = slave->affineTransform;
-    if(tfo.size() >= 16){
-      for (unsigned int i = 0; i < master->getNumMeshVertices(); ++i) {
-        MVertex *vs = master->getMeshVertex(i);
-        double ps[4] = {vs->x(), vs->y(), vs->z(), 1.};
-        double res[4] = {0., 0., 0., 0.};
-        int idx = 0;
-        for(int i = 0; i < 4; i++)
-          for(int j = 0; j < 4; j++)
-            res[i] +=  tfo[idx++] * ps[j];
-        SPoint3 p3 (res[0], res[1], res[2]);
-        SPoint2 p2 = slave->parFromPoint(p3);
-        GPoint gp = slave->point(p2);
-        MVertex *vt = rtree.find(gp.x(), gp.y(), gp.z());
-        if (!vt) Msg::Error("Couldn't find a vertex for updating periodicity");
-        else v2v[vt] = vs;
+    GFace *tgt = *it;
+    GFace *src = dynamic_cast<GFace*>(tgt->meshMaster());
+    if (src != NULL && src != tgt) {
+
+      Msg::Info("Constructing high order periodicity for face connection %d - %d",
+                tgt->tag(),src->tag());
+      
+      std::map<MVertex*,MVertex*> &p2p = tgt->correspondingHOPoints;
+      p2p.clear();
+      
+      std::map<MFace,MElement*,Less_Face> srcFaces;
+    
+      for (unsigned int i=0;i<src->getNumMeshElements();++i) {
+        MElement* srcElmt  = src->getMeshElement(i);
+        int nbVtcs = 0;
+        if (dynamic_cast<MTriangle*>   (srcElmt)) nbVtcs = 3;
+        if (dynamic_cast<MQuadrangle*> (srcElmt)) nbVtcs = 4;
+        std::vector<MVertex*> vtcs;
+        for (int iVtx=0;iVtx<nbVtcs;iVtx++) {
+          vtcs.push_back(srcElmt->getVertex(iVtx));
+        }
+        srcFaces[MFace(vtcs)] = srcElmt;
+      }
+    
+      for (unsigned int i=0;i<tgt->getNumMeshElements();++i) {
+      
+        MElement* tgtElmt = tgt->getMeshElement(i);
+        Msg::Info("Checking element %d in face %d",i,tgt->tag());
+        
+        int nbVtcs = 0;
+        if (dynamic_cast<MTriangle*>   (tgtElmt)) nbVtcs = 3;
+        if (dynamic_cast<MQuadrangle*> (tgtElmt)) nbVtcs = 4;
+        std::vector<MVertex*> vtcs;
+        for (int iVtx=0;iVtx<nbVtcs;iVtx++) {
+          MVertex* vtx = tgtElmt->getVertex(iVtx);
+          GEntity* ge = vtx->onWhat();
+          if (ge->meshMaster() == ge) throw;
+          std::map<MVertex*,MVertex*>& v2v = ge->correspondingVertices;
+          vtcs.push_back(v2v[vtx]);
+        }
+        
+        std::map<MFace,MElement*>::iterator srcIter = srcFaces.find(MFace(vtcs));
+        if (srcIter == srcFaces.end()) {
+          std::ostringstream faceDef;
+          for (int iVtx=0;iVtx<nbVtcs;iVtx++) faceDef << vtcs[iVtx]->getNum() << " ";
+          Msg::Error("Cannot find periodic counterpart of face %s in face %d "
+                     "connected to %d",faceDef.str().c_str(),
+                     tgt->tag(),src->tag());
+        }
+        else {
+          MElement* srcElmt = srcIter->second;
+          for (int i=nbVtcs;i<srcElmt->getNumVertices();i++) {
+            p2p[tgtElmt->getVertex(i)] = srcElmt->getVertex(i);
+          }
+        }
       }
     }
   }
+  Msg::Info("Finalized high order topology of periodic connections");
 }
 
 void SetOrder1(GModel *m,  bool onlyVisible)
diff --git a/Mesh/meshGEdge.cpp b/Mesh/meshGEdge.cpp
index f290f990c7c13444ca6c387516afaac6ad8a76af..59f2a8894ba02fedfd4476fba52914b6ce32a801 100644
--- a/Mesh/meshGEdge.cpp
+++ b/Mesh/meshGEdge.cpp
@@ -303,40 +303,6 @@ void copyMesh(GEdge *from, GEdge *to, int direction)
   }
 }
 
-bool correctPeriodicity(MVertex*,MVertex*,const std::vector<double>&);
-bool correctVertexPeriodicity(GEntity*);
-
-
-bool correctPeriodicity(GEdge* tgt) {
-
-  if (correctVertexPeriodicity(tgt)) {
-    
-    bool check = true;
-    
-    GEdge* src = dynamic_cast<GEdge*> (tgt->meshMaster());
-
-    if (!src) throw;
-    
-    const std::vector<double>& tfo = src->affineTransform;    
-
-    std::vector<MLine*>::iterator sIter = src->lines.begin();
-    std::vector<MLine*>::iterator tIter = tgt->lines.begin();
-    
-    for (;sIter!=src->lines.end();++sIter,++tIter) {
-
-      MLine* src = *sIter;
-      MLine* tgt = *sIter;
-
-      for (int iVtx=2;iVtx<tgt->getNumVertices();iVtx++) {
-        check = check && 
-          correctPeriodicity(tgt->getVertex(iVtx),src->getVertex(iVtx),tfo);
-      }
-    }
-  }
-  return false;
-}
-
-
 void deMeshGEdge::operator() (GEdge *ge)
 {
   if(ge->geomType() == GEntity::DiscreteCurve && !CTX::instance()->meshDiscrete)
diff --git a/Mesh/meshGEntity.cpp b/Mesh/meshGEntity.cpp
deleted file mode 100644
index 2a8880829e3c7ab4a875f7a9a4f1445c3d28ad6f..0000000000000000000000000000000000000000
--- a/Mesh/meshGEntity.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-// Gmsh - Copyright (C) 1997-2016 C. Geuzaine, J.-F. Remacle
-//
-// See the LICENSE.txt file for license information. Please report all
-// bugs and problems to the public mailing list <gmsh@onelab.info>.
-//
-// Contributor(s):
-//   Koen Hillewaert
-
-#include "GEntity.h"
-#include "MVertex.h"
-
-// -----------------------------------------------------------------------------
-
-bool correctPeriodicity(MVertex* tgt,
-                        MVertex* src,
-                        const std::vector<double>& tfo) {
-  
-  double ps[4] = {src->x(),src->y(),src->z(),1};
-  double pt[4] = {tgt->x(),tgt->y(),tgt->z(),1};
-  int idx = 0;
-  for(int i = 0; i < 4; i++)
-    for(int j = 0; j < 4; j++)
-      ps[j] +=  tfo[idx++] * pt[i];
-  
-  for (int i=0;i<4;i++) ps[i] *= 0.5;
-  for (int i=0;i<4;i++) pt[i] = 0;
-
-  src->x() = ps[0];
-  src->y() = ps[1];
-  src->z() = ps[2];
-  
-  idx = 0;
-  for(int i = 0; i < 4; i++)
-    for(int j = 0; j < 4; j++)
-      pt[i] +=  tfo[idx++] * ps[j];
-  
-  tgt->x() = pt[0];
-  tgt->y() = pt[1];
-  tgt->z() = pt[2];
-
-  return true;
-}
-
-// -----------------------------------------------------------------------------
-
-bool correctVertexPeriodicity(GEntity* tgt) {
-  
-  if (tgt->meshMaster() == NULL) return false;
-  bool check = true;
-  std::map<MVertex*,MVertex*>::iterator vIter = tgt->correspondingVertices.begin();
-  for (;vIter!=tgt->correspondingVertices.end();++vIter) {
-    check = check && correctPeriodicity(vIter->first,
-                                        vIter->second,
-                                        tgt->affineTransform);
-    
-  }
-  return check;
-}
-
-// -----------------------------------------------------------------------------
diff --git a/Mesh/meshGFace.cpp b/Mesh/meshGFace.cpp
index 5cd6ec89ec327af05851de5343ce977c771c22dc..53b4b42faf15526e94aae633e789dde7ba96d20c 100644
--- a/Mesh/meshGFace.cpp
+++ b/Mesh/meshGFace.cpp
@@ -380,60 +380,6 @@ static void copyMesh(GFace *source, GFace *target)
   }
 }
 
-
-bool correctPeriodicity(MVertex*,MVertex*,const std::vector<double>&);
-bool correctVertexPeriodicity(GEntity*);
-
-template<class MElementType>
-bool correctPeriodicity(std::vector<MElementType*>& tgtList,
-                        std::vector<MElementType*>& srcList,
-                        const std::vector<double>& tfo,
-                        std::set<MVertex*>& corrected) {
-
-  bool check = true;
-
-  typename std::vector<MElementType*>::iterator sIter = srcList.begin();
-  typename std::vector<MElementType*>::iterator tIter = tgtList.begin();
-  
-  for (;sIter!=srcList.end();++sIter,++tIter) {
-    
-    MElementType* src = *sIter;
-    MElementType* tgt = *sIter;
-    
-    for (int iVtx=0;iVtx<tgt->getNumVertices();iVtx++) {
-
-      MVertex* tgtVtx = tgt->getVertex(iVtx);
-      MVertex* srcVtx = src->getVertex(iVtx);
-
-      if (corrected.find(tgtVtx) != corrected.end()) {
-
-        check = check && correctPeriodicity(tgtVtx,srcVtx,tfo);
-        if (check) corrected.insert(tgtVtx);
-      }
-    }
-  }
-  return check;
-}
-
-bool correctPeriodicity(GFace* tgt) {
-
-  if (!correctVertexPeriodicity(tgt)) return false;
-    
-    
-  GFace* src = dynamic_cast<GFace*> (tgt->meshMaster());
-  if (!src) throw;
-  
-  const std::vector<double>& tfo = src->affineTransform;    
-
-  std::set<MVertex*> corr(tgt->mesh_vertices.begin(),tgt->mesh_vertices.end());
-  
-  if (!correctPeriodicity(tgt->triangles  ,src->triangles  ,tfo,corr)) return false;
-  if (!correctPeriodicity(tgt->quadrangles,src->quadrangles,tfo,corr)) return false;
-  if (!correctPeriodicity(tgt->polygons   ,src->polygons   ,tfo,corr)) return false;
-  
-  return true;
-}
-
 void fourthPoint(double *p1, double *p2, double *p3, double *p4)
 {
   double c[3];
diff --git a/contrib/HighOrderMeshOptimizer/OptHomPeriodicity.cpp b/contrib/HighOrderMeshOptimizer/OptHomPeriodicity.cpp
index e6cb5ab968b2354cc72615f5760f8d9956f94d00..95b940afbba117a39612d3a8a096291e09c1b48d 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomPeriodicity.cpp
+++ b/contrib/HighOrderMeshOptimizer/OptHomPeriodicity.cpp
@@ -49,100 +49,192 @@ OptHomPeriodicity::OptHomPeriodicity(std::vector<GEntity*> &entities)
 
 void OptHomPeriodicity::fixPeriodicity()
 {
+  Msg::Info("Correcting high order optimization for periodic connections");
   _relocateMasterVertices();
   _copyBackMasterVertices();
+  Msg::Info("Finished correcting high order optimization for periodic connections");
 }
 
 void OptHomPeriodicity::_relocateMasterVertices()
 {
+  
   std::multimap<GEntity*, GEntity*>::iterator it;
   for (it = _master2slave.begin(); it != _master2slave.end(); ++it) {
-    if (it->first->dim() == 2) {
-      GFace *master = dynamic_cast<GFace*>(it->first);
-      GFace *slave = dynamic_cast<GFace*>(it->second);
-      std::vector<double> tfo = _inverse(slave->affineTransform);
-
-      std::map<MVertex*, MVertex*> &vertS2M = slave->correspondingVertices;
-      std::map<MVertex*, MVertex*>::iterator vit;
-      for (vit = vertS2M.begin(); vit != vertS2M.end(); ++vit) {
-        GPoint p = _transform(vit->first, master, tfo);
-        MFaceVertex *v = dynamic_cast<MFaceVertex*>(vit->second);
-        SPoint3 p3 ((v->x()+p.x())/2, (v->y()+p.y())/2, (v->z()+p.z())/2);
-        SPoint2 p2 = master->parFromPoint(p3);
-        GPoint gp = master->point(p2);
-        v->setXYZ(gp.x(), gp.y(), gp.z());
-        v->setParameter(0, gp.u());
-        v->setParameter(1, gp.v());
-      }
-    }
-    else {
-      GEdge *master = dynamic_cast<GEdge*>(it->first);
-      int numSlave = _master2slave.count(master);
 
-      for (int i = 0; i < numSlave; ++i) {
-        if (i > 0) ++it;
-        GEntity *slave = it->second;
-        std::vector<double> tfo = _inverse(slave->affineTransform);
+    switch (it->first->dim()) {
 
+    case 2:
+      {
+        GFace *master = dynamic_cast<GFace*>(it->first);
+        GFace *slave = dynamic_cast<GFace*>(it->second);
+        std::vector<double> tfo = _inverse(slave->affineTransform);
+        
         std::map<MVertex*, MVertex*> &vertS2M = slave->correspondingVertices;
         std::map<MVertex*, MVertex*>::iterator vit;
         for (vit = vertS2M.begin(); vit != vertS2M.end(); ++vit) {
-          GPoint p = _transform(vit->first, master, tfo);
-          MVertex *v = vit->second;
-          v->setXYZ(v->x()+p.x(), v->y()+p.y(), v->z()+p.z());
+          MFaceVertex *v = dynamic_cast<MFaceVertex*>(vit->second);
+          if (v && v->onWhat() == master) { // treat only nodes classified on the surface
+            GPoint p = _transform(vit->first, master, tfo);
+            SPoint3 p3 ((v->x()+p.x())/2, (v->y()+p.y())/2, (v->z()+p.z())/2);
+            SPoint2 p2 = master->parFromPoint(p3);
+            GPoint gp = master->point(p2);
+            v->setXYZ(gp.x(), gp.y(), gp.z());
+            v->setParameter(0, gp.u());
+            v->setParameter(1, gp.v());
+          }
+        }
+        
+        std::map<MVertex*, MVertex*> &pointS2M = slave->correspondingHOPoints;
+        for (vit = pointS2M.begin(); vit != pointS2M.end(); ++vit) {
+          MFaceVertex *v = dynamic_cast<MFaceVertex*>(vit->second);
+          if (v && v->onWhat() == master) {
+            GPoint p = _transform(vit->first, master, tfo);
+            SPoint3 p3 ((v->x()+p.x())/2, (v->y()+p.y())/2, (v->z()+p.z())/2);
+            SPoint2 p2 = master->parFromPoint(p3);
+            GPoint gp = master->point(p2);
+            v->setXYZ(gp.x(), gp.y(), gp.z());
+            v->setParameter(0, gp.u());
+            v->setParameter(1, gp.v());
+          }
         }
+        break;
       }
-
-      double coeff = 1./(1+numSlave);
-      for (unsigned int k = 0; k < master->getNumMeshVertices(); ++k) {
-        MEdgeVertex *v = dynamic_cast<MEdgeVertex*>(master->getMeshVertex(k));
-        SPoint3 p3 (v->x()*coeff, v->y()*coeff, v->z()*coeff);
-        double u = master->parFromPoint(p3);
-        GPoint gp = master->point(u);
-        v->setXYZ(gp.x(), gp.y(), gp.z());
-        v->setParameter(0, gp.u());
+    case 3:
+      {
+        GEdge *master = dynamic_cast<GEdge*>(it->first);
+        int numSlave = _master2slave.count(master);
+        
+        for (int i = 0; i < numSlave; ++i) {
+          if (i > 0) ++it;
+          GEntity *slave = it->second;
+          std::vector<double> tfo = _inverse(slave->affineTransform);
+          std::map<MVertex*, MVertex*>::iterator vit;
+          
+          std::map<MVertex*, MVertex*> &vertS2M = slave->correspondingVertices;
+          for (vit = vertS2M.begin(); vit != vertS2M.end(); ++vit) {
+            MEdgeVertex* v = dynamic_cast<MEdgeVertex*> (vit->second);
+            if (v && v->onWhat() == master) {
+              GPoint p = _transform(vit->first, master, tfo);
+              v->setXYZ(v->x()+p.x(), v->y()+p.y(), v->z()+p.z());
+            }
+          }
+          std::map<MVertex*, MVertex*> &pointS2M = slave->correspondingHOPoints;
+          for (vit = pointS2M.begin(); vit != pointS2M.end(); ++vit) {
+            MEdgeVertex* v = dynamic_cast<MEdgeVertex*> (vit->second);
+            if (v && v->onWhat() == master) {
+              GPoint p = _transform(vit->first, master, tfo);
+              v->setXYZ(v->x()+p.x(), v->y()+p.y(), v->z()+p.z());
+            }
+          }
+        }
+        
+        double coeff = 1./(1+numSlave);
+        for (unsigned int k = 0; k < master->getNumMeshVertices(); ++k) {
+          MEdgeVertex *v = dynamic_cast<MEdgeVertex*>(master->getMeshVertex(k));
+          if (v && v->onWhat() == master) {
+            SPoint3 p3 (v->x()*coeff, v->y()*coeff, v->z()*coeff);
+            double u = master->parFromPoint(p3);
+            GPoint gp = master->point(u);
+            v->setXYZ(gp.x(), gp.y(), gp.z());
+            v->setParameter(0, gp.u());
+          }
+        }
+        break;
       }
     }
   }
 }
 
+#include <iostream>
+
 void OptHomPeriodicity::_copyBackMasterVertices()
 {
   std::multimap<GEntity*, GEntity*>::iterator it;
   for (it = _master2slave.begin(); it != _master2slave.end(); ++it) {
-    if (it->first->dim() == 2) {
-      GFace *master = dynamic_cast<GFace*>(it->first);
-      GFace *slave = dynamic_cast<GFace*>(it->second);
-      std::vector<double> tfo = slave->affineTransform;
-
-      std::map<MVertex*, MVertex*> &vertS2M = slave->correspondingVertices;
-      std::map<MVertex*, MVertex*>::iterator vit;
-      for (vit = vertS2M.begin(); vit != vertS2M.end(); ++vit) {
-        GPoint p = _transform(vit->second, slave, tfo);
-        MFaceVertex *v = dynamic_cast<MFaceVertex*>(vit->first);
-        v->setXYZ(p.x(), p.y(), p.z());
-        v->setParameter(0, p.u());
-        v->setParameter(1, p.v());
+    
+    switch (it->first->dim()) {
+    case 2:
+      {
+        GFace *master = dynamic_cast<GFace*>(it->first);
+        GFace *slave = dynamic_cast<GFace*>(it->second);
+        
+        Msg::Info("Copying master vertices from face %d to %d",
+                  master->tag(),slave->tag());
+
+        std::cout << "Copying face " << master->tag() 
+                  << " to " << slave->tag() << std::endl;
+
+        const std::vector<double>& tfo = slave->affineTransform;
+        std::map<MVertex*, MVertex*>::iterator vit;
+        
+        std::map<MVertex*, MVertex*> &vertS2M = slave->correspondingVertices;
+        for (vit = vertS2M.begin(); vit != vertS2M.end(); ++vit) {
+          MFaceVertex *sv = dynamic_cast<MFaceVertex*>(vit->first);
+          MFaceVertex *mv = dynamic_cast<MFaceVertex*>(vit->second);
+          if (mv && mv->onWhat() == master) {
+            GPoint p = _transform(mv, slave, tfo);
+            sv->setXYZ(p.x(), p.y(), p.z());
+            sv->setParameter(0, p.u());
+            sv->setParameter(1, p.v());
+          }
+        }
+        
+        std::map<MVertex*, MVertex*> &pointS2M = slave->correspondingHOPoints;
+        for (vit = pointS2M.begin(); vit != pointS2M.end(); ++vit) {
+          MFaceVertex *sv = dynamic_cast<MFaceVertex*>(vit->first);
+          MFaceVertex *mv = dynamic_cast<MFaceVertex*>(vit->second);
+          if (mv && mv->onWhat() == master) {
+            GPoint p = _transform(mv, slave, tfo);
+            sv->setXYZ(p.x(), p.y(), p.z());
+            sv->setParameter(0, p.u());
+            sv->setParameter(1, p.v());
+          }
+        }
+        break;
       }
-    }
-    else {
-      GEdge *master = dynamic_cast<GEdge*>(it->first);
-      GEdge *slave = dynamic_cast<GEdge*>(it->second);
-      std::vector<double> tfo = slave->affineTransform;
-
-      std::map<MVertex*, MVertex*> &vertS2M = slave->correspondingVertices;
-      std::map<MVertex*, MVertex*>::iterator vit;
-      for (vit = vertS2M.begin(); vit != vertS2M.end(); ++vit) {
-        GPoint p = _transform(vit->second, slave, tfo);
-        MEdgeVertex *v = dynamic_cast<MEdgeVertex*>(vit->first);
-        v->setXYZ(p.x(), p.y(), p.z());
-        v->setParameter(0, p.u());
+    case 3:
+      {
+        GEdge *master = dynamic_cast<GEdge*>(it->first);
+        GEdge *slave = dynamic_cast<GEdge*>(it->second);
+        
+        Msg::Info("Copying master vertices from edge %d to %d",
+                  master->tag(),slave->tag());
+        
+        std::cout << "Copying edge " << master->tag() 
+                  << " to " << slave->tag() << std::endl;
+
+        
+        const std::vector<double> tfo = slave->affineTransform;
+        std::map<MVertex*, MVertex*>::iterator vit;
+      
+        std::map<MVertex*, MVertex*> &vertS2M = slave->correspondingVertices;
+        for (vit = vertS2M.begin(); vit != vertS2M.end(); ++vit) {
+          GPoint p = _transform(vit->second, slave, tfo);
+          MEdgeVertex *sv = dynamic_cast<MEdgeVertex*>(vit->first);
+          MEdgeVertex *mv = dynamic_cast<MEdgeVertex*>(vit->second);
+          if (mv && mv->onWhat() == master) {
+            sv->setXYZ(p.x(), p.y(), p.z());
+            sv->setParameter(0, p.u());
+          }
+        }
+
+        std::map<MVertex*, MVertex*> &pointS2M = slave->correspondingHOPoints;
+        for (vit = pointS2M.begin(); vit != pointS2M.end(); ++vit) {
+          GPoint p = _transform(vit->second, slave, tfo);
+          MEdgeVertex *sv = dynamic_cast<MEdgeVertex*>(vit->first);
+          MEdgeVertex *mv = dynamic_cast<MEdgeVertex*>(vit->second);
+          if (mv && mv->onWhat() == master) {
+            sv->setXYZ(p.x(), p.y(), p.z());
+            sv->setParameter(0, p.u());
+          }
+        }
+        break;
       }
     }
   }
 }
 
-GPoint OptHomPeriodicity::_transform(MVertex *vsource, GEntity *target, std::vector<double> &tfo)
+GPoint OptHomPeriodicity::_transform(MVertex *vsource, GEntity *target, const std::vector<double> &tfo)
 {
   double ps[4] = {vsource->x(), vsource->y(), vsource->z(), 1.};
   double res[4] = {0., 0., 0., 0.};
@@ -167,7 +259,7 @@ GPoint OptHomPeriodicity::_transform(MVertex *vsource, GEntity *target, std::vec
   }
 }
 
-std::vector<double> OptHomPeriodicity::_inverse(std::vector<double> &tfo)
+std::vector<double> OptHomPeriodicity::_inverse(const std::vector<double> &tfo)
 {
   // Note that the last row of tfo must be (0 0 0 1)...
   std::vector<double> result(16);
diff --git a/contrib/HighOrderMeshOptimizer/OptHomPeriodicity.h b/contrib/HighOrderMeshOptimizer/OptHomPeriodicity.h
index 919c40869e34f83fd587e5459c5ccc94d8e03403..c156507fc96666bb9d3519ea5a6a5e68112b1e84 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomPeriodicity.h
+++ b/contrib/HighOrderMeshOptimizer/OptHomPeriodicity.h
@@ -52,8 +52,8 @@ private:
   void _relocateMasterVertices();
   void _copyBackMasterVertices();
 
-  static GPoint _transform(MVertex*, GEntity*, std::vector<double>&);
-  static std::vector<double> _inverse(std::vector<double>&);
+  static GPoint _transform(MVertex*, GEntity*, const std::vector<double>&);
+  static std::vector<double> _inverse(const std::vector<double>&);
 };
 
 #endif