diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7450dd2badc63e2e7572073f67f0a96739c93cc7..410bf5d0abbf461cfe3c8ceae757da0f7a627e92 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -144,6 +144,13 @@ set(GMSH_API
   contrib/HighOrderMeshOptimizer/OptHOM.h contrib/HighOrderMeshOptimizer/OptHomMesh.h
   contrib/HighOrderMeshOptimizer/OptHomRun.h contrib/HighOrderMeshOptimizer/ParamCoord.h
   contrib/HighOrderMeshOptimizer/OptHomFastCurving.h contrib/HighOrderMeshOptimizer/SuperEl.h
+  contrib/HighOrderMeshOptimizer/OptHomObjContribScaledJac.h
+  contrib/HighOrderMeshOptimizer/OptHomObjContribMetricMin.h
+  contrib/HighOrderMeshOptimizer/OptHomObjContribCADDist.h
+  contrib/MeshOptimizer/MeshOptPatch.h contrib/MeshOptimizer/MeshOpt.h
+  contrib/MeshOptimizer/MeshOptCommon.h contrib/MeshOptimizer/MeshOptimizer.h
+  contrib/MeshOptimizer/MeshOptObjContribFunc.h contrib/MeshOptimizer/MeshOptObjContrib.h
+  contrib/MeshOptimizer/MeshOptObjectiveFunction.h contrib/MeshOptimizer/MeshOptVertexCoord.h 
   contrib/MathEx/mathex.h)
 
 get_property(IAMCHILD DIRECTORY  PROPERTY PARENT_DIRECTORY)
@@ -636,6 +643,8 @@ endif(ENABLE_DINTEGRATION)
 if(ENABLE_OPTHOM)
   add_subdirectory(contrib/HighOrderMeshOptimizer)
   include_directories(contrib/HighOrderMeshOptimizer)
+  add_subdirectory(contrib/MeshOptimizer)
+  include_directories(contrib/MeshOptimizer)
   set_config_option(HAVE_OPTHOM "OptHom")
 endif(ENABLE_OPTHOM)
 
diff --git a/contrib/HighOrderMeshOptimizer/OptHomObjContribCADDist.h b/contrib/HighOrderMeshOptimizer/OptHomObjContribCADDist.h
new file mode 100644
index 0000000000000000000000000000000000000000..d4b8d1e2aa34141bdfb704af22de7ac86d105e65
--- /dev/null
+++ b/contrib/HighOrderMeshOptimizer/OptHomObjContribCADDist.h
@@ -0,0 +1,131 @@
+// TODO: Copyright
+
+#ifndef _OPTHOMOBJCONTRIBCADDIST_H_
+#define _OPTHOMOBJCONTRIBCADDIST_H_
+
+#include "MeshOptObjContrib.h"
+
+
+template<class FuncType>
+class ObjContribCADDist : public ObjContrib, public FuncType
+{
+public:
+  ObjContribCADDist(double weight);
+  virtual ~ObjContribCADDist() {}
+  virtual void initialize(Patch *mesh);
+  virtual bool fail() { return false; }
+  virtual bool addContrib(double &Obj, alglib::real_1d_array &gradObj);
+  virtual void updateParameters() { FuncType::updateParameters(_min, _max); }
+  virtual bool targetReached() { return FuncType::targetReached(_min, _max); }
+  virtual bool stagnated() { return FuncType::stagnated(_min, _max); }
+  virtual void updateMinMax();
+  virtual void updateResults(MeshOptResults &res) const;
+
+protected:
+  Patch *_mesh;
+  double _weight;
+  std::vector<int> _nBezEl;                                                             // Number of Bezier poly. and for an el.
+  inline const int &nBezEl(int iEl) { return _nBezEl[iEl]; }
+  inline int indGSJ(int iEl, int l, int iPC) { return iPC*_nBezEl[iEl]+l; }
+  void metricMinAndGradients(int iEl, std::vector<double> &sJ, std::vector<double> &gSJ);
+};
+
+
+template<class FuncType>
+ObjContribCADDist<FuncType>::ObjContribCADDist(double weight) :
+  ObjContrib("MetricMin", FuncType::getNamePrefix()+"MetricMin"),
+  _mesh(0), _weight(weight)
+{
+}
+
+
+template<class FuncType>
+void ObjContribCADDist<FuncType>::initialize(Patch *mesh)
+{
+  _mesh = mesh;
+
+  // Initialize _nBezEl
+  _nBezEl.resize(_mesh->nEl());
+  for (int iEl=0; iEl<_mesh->nEl(); iEl++)
+   _nBezEl[iEl] = _mesh->el(iEl)->getJacobianFuncSpace()->getNumJacNodes();
+
+  updateMinMax();
+  FuncType::initialize(_min, _max);
+}
+
+
+template<class FuncType>
+bool ObjContribCADDist<FuncType>::addContrib(double &Obj, alglib::real_1d_array &gradObj)
+{
+  _min = BIGVAL;
+  _max = -BIGVAL;
+
+  for (int iEl = 0; iEl < _mesh->nEl(); iEl++) {
+    std::vector<double> mM(_nBezEl[iEl]);                       // Min. of Metric
+    std::vector<double> gMM(_nBezEl[iEl]*_mesh->nPCEl(iEl));    // Dummy gradients of metric min.
+    metricMinAndGradients(iEl, mM, gMM);
+    for (int l = 0; l < _nBezEl[iEl]; l++) {                    // Add contribution for each Bezier coeff.
+      Obj += _weight * FuncType::compute(mM[l]);
+      for (int iPC = 0; iPC < _mesh->nPCEl(iEl); iPC++)
+        gradObj[_mesh->indPCEl(iEl,iPC)] +=
+            _weight * FuncType::computeDiff(mM[l], gMM[indGSJ(iEl, l, iPC)]);
+      _min = std::min(_min, mM[l]);
+      _max = std::max(_max, mM[l]);
+    }
+  }
+
+  return true;
+}
+
+
+template<class FuncType>
+void ObjContribCADDist<FuncType>::updateMinMax()
+{
+  _min = BIGVAL;
+  _max = -BIGVAL;
+
+  for (int iEl = 0; iEl < _mesh->nEl(); iEl++) {
+    std::vector<double> mM(_nBezEl[iEl]);                         // Min. of Metric
+    std::vector<double> dumGMM(_nBezEl[iEl]*_mesh->nPCEl(iEl));   // Dummy gradients of metric min.
+    metricMinAndGradients(iEl, mM, dumGMM);
+    for (int l = 0; l < _nBezEl[iEl]; l++) {                      // Check each Bezier coeff.
+      _min = std::min(_min, mM[l]);
+      _max = std::max(_max, mM[l]);
+    }
+  }
+}
+
+
+template<class FuncType>
+void ObjContribCADDist<FuncType>::updateResults(MeshOptResults &res) const
+{
+  res.minMetricMin = std::min(_min, res.minMetricMin);
+  res.maxMetricMin = std::max(_max, res.maxMetricMin);
+}
+
+
+//bool MeshOpt::addBndObjGrad(double factor, double &Obj, alglib::real_1d_array &gradObj)
+//{
+//  maxDistCAD = 0.0;
+//
+//  std::vector<double> gradF;
+//  double DISTANCE = 0.0;
+//  for (int iEl = 0; iEl < mesh.nEl(); iEl++) {
+//    double f;
+//    if (mesh.bndDistAndGradients(iEl, f, gradF, geomTol)) {
+//      maxDistCAD = std::max(maxDistCAD,f);
+//      DISTANCE += f;
+//      Obj += f * factor;
+//      for (size_t i = 0; i < mesh.nPCEl(iEl); ++i){
+//        gradObj[mesh.indPCEl(iEl, i)] += gradF[i] * factor;
+//  //  printf("gradf[%d] = %12.5E\n",i,gradF[i]*factor);
+//      }
+//    }
+//  }
+//  //  printf("DIST = %12.5E\n",DISTANCE);
+//  return true;
+//
+//}
+
+
+#endif /* _OPTHOMOBJCONTRIBCADDIST_H_ */
diff --git a/contrib/HighOrderMeshOptimizer/OptHomObjContribMetricMin.h b/contrib/HighOrderMeshOptimizer/OptHomObjContribMetricMin.h
new file mode 100644
index 0000000000000000000000000000000000000000..e6d98681b64c31ed917b0499e83795bdd4665d30
--- /dev/null
+++ b/contrib/HighOrderMeshOptimizer/OptHomObjContribMetricMin.h
@@ -0,0 +1,164 @@
+// TODO: Copyright
+
+#ifndef _OPTHOMOBJCONTRIBMETRICMIN_H_
+#define _OPTHOMOBJCONTRIBMETRICMIN_H_
+
+#include "MeshOptPatch.h"
+#include "MeshOptObjContrib.h"
+
+
+template<class FuncType>
+class ObjContribMetricMin : public ObjContrib, public FuncType
+{
+public:
+  ObjContribMetricMin(double weight);
+  virtual ~ObjContribMetricMin() {}
+  virtual void initialize(Patch *mesh);
+  virtual bool fail() { return false; }
+  virtual bool addContrib(double &Obj, alglib::real_1d_array &gradObj);
+  virtual void updateParameters() { FuncType::updateParameters(_min, _max); }
+  virtual bool targetReached() { return FuncType::targetReached(_min, _max); }
+  virtual bool stagnated() { return FuncType::stagnated(_min, _max); }
+  virtual void updateMinMax();
+  virtual void updateResults(MeshOptResults &res) const;
+
+protected:
+  Patch *_mesh;
+  double _weight;
+  std::vector<int> _nBezEl;                                                             // Number of Bezier poly. and for an el.
+  inline const int &nBezEl(int iEl) { return _nBezEl[iEl]; }
+  inline int indGSJ(int iEl, int l, int iPC) { return iPC*_nBezEl[iEl]+l; }
+  void metricMinAndGradients(int iEl, std::vector<double> &sJ, std::vector<double> &gSJ);
+};
+
+
+template<class FuncType>
+ObjContribMetricMin<FuncType>::ObjContribMetricMin(double weight) :
+  ObjContrib("MetricMin", FuncType::getNamePrefix()+"MetricMin"),
+  _mesh(0), _weight(weight)
+{
+}
+
+
+template<class FuncType>
+void ObjContribMetricMin<FuncType>::initialize(Patch *mesh)
+{
+  _mesh = mesh;
+
+  // Initialize _nBezEl
+  _nBezEl.resize(_mesh->nEl());
+  for (int iEl=0; iEl<_mesh->nEl(); iEl++)
+   _nBezEl[iEl] = _mesh->el(iEl)->getJacobianFuncSpace()->getNumJacNodes();
+
+  updateMinMax();
+  FuncType::initialize(_min, _max);
+}
+
+
+template<class FuncType>
+bool ObjContribMetricMin<FuncType>::addContrib(double &Obj, alglib::real_1d_array &gradObj)
+{
+  _min = BIGVAL;
+  _max = -BIGVAL;
+
+  for (int iEl = 0; iEl < _mesh->nEl(); iEl++) {
+    std::vector<double> mM(_nBezEl[iEl]);                       // Min. of Metric
+    std::vector<double> gMM(_nBezEl[iEl]*_mesh->nPCEl(iEl));    // Dummy gradients of metric min.
+    metricMinAndGradients(iEl, mM, gMM);
+    for (int l = 0; l < _nBezEl[iEl]; l++) {                    // Add contribution for each Bezier coeff.
+      Obj += _weight * FuncType::compute(mM[l]);
+      for (int iPC = 0; iPC < _mesh->nPCEl(iEl); iPC++)
+        gradObj[_mesh->indPCEl(iEl,iPC)] +=
+            _weight * FuncType::computeDiff(mM[l], gMM[indGSJ(iEl, l, iPC)]);
+      _min = std::min(_min, mM[l]);
+      _max = std::max(_max, mM[l]);
+    }
+  }
+
+  return true;
+}
+
+
+template<class FuncType>
+void ObjContribMetricMin<FuncType>::updateMinMax()
+{
+  _min = BIGVAL;
+  _max = -BIGVAL;
+
+  for (int iEl = 0; iEl < _mesh->nEl(); iEl++) {
+    std::vector<double> mM(_nBezEl[iEl]);                         // Min. of Metric
+    std::vector<double> dumGMM(_nBezEl[iEl]*_mesh->nPCEl(iEl));   // Dummy gradients of metric min.
+    metricMinAndGradients(iEl, mM, dumGMM);
+    for (int l = 0; l < _nBezEl[iEl]; l++) {                      // Check each Bezier coeff.
+      _min = std::min(_min, mM[l]);
+      _max = std::max(_max, mM[l]);
+    }
+  }
+}
+
+
+template<class FuncType>
+void ObjContribMetricMin<FuncType>::updateResults(MeshOptResults &res) const
+{
+  res.minMetricMin = std::min(_min, res.minMetricMin);
+  res.maxMetricMin = std::max(_max, res.maxMetricMin);
+}
+
+
+template<class FuncType>
+void ObjContribMetricMin<FuncType>::metricMinAndGradients(int iEl, std::vector<double> &lambda,
+                                                                 std::vector<double> &gradLambda)
+{
+  const JacobianBasis *jacBasis = _mesh->el(iEl)->getJacobianFuncSpace();
+  const int &numJacNodes = jacBasis->getNumJacNodes();
+  const int &numMapNodes = jacBasis->getNumMapNodes();
+  const int &numPrimMapNodes = jacBasis->getNumPrimMapNodes();
+  fullVector<double> lambdaJ(numJacNodes), lambdaB(numJacNodes);
+  fullMatrix<double> gradLambdaJ(numJacNodes, 2*numMapNodes);
+  fullMatrix<double> gradLambdaB(numJacNodes, 2*numMapNodes);
+
+  // Coordinates of nodes
+  fullMatrix<double> nodesXYZ(numMapNodes, 3), nodesXYZStraight(numPrimMapNodes, 3);
+  for (int i = 0; i < numMapNodes; i++) {
+    const int &iVi = _mesh->el2V(iEl, i);
+    nodesXYZ(i, 0) = _mesh->xyz(iVi).x();
+    nodesXYZ(i, 1) = _mesh->xyz(iVi).y();
+    nodesXYZ(i, 2) = _mesh->xyz(iVi).z();
+    if (i < numPrimMapNodes) {
+      nodesXYZStraight(i, 0) = _mesh->ixyz(iVi).x();
+      nodesXYZStraight(i, 1) = _mesh->ixyz(iVi).y();
+      nodesXYZStraight(i, 2) = _mesh->ixyz(iVi).z();
+    }
+  }
+
+  jacBasis->getMetricMinAndGradients(nodesXYZ, nodesXYZStraight, lambdaJ, gradLambdaJ);
+
+  lambdaB = lambdaJ;
+  gradLambdaB = gradLambdaJ;
+
+  int iPC = 0;
+  std::vector<SPoint3> gXyzV(numJacNodes);
+  std::vector<SPoint3> gUvwV(numJacNodes);
+  for (int l = 0; l < numJacNodes; l++) {
+    lambda[l] = lambdaB(l);
+  }
+  for (int i = 0; i < numMapNodes; i++) {
+    const int &iFVi = _mesh->el2FV(iEl, i);
+    if (iFVi >= 0) {
+      for (int l = 0; l < numJacNodes; l++) {
+        gXyzV [l] = SPoint3(gradLambdaB(l, i),
+                            gradLambdaB(l, i+numMapNodes),/*BDB(l,i+2*nbNod)*/ 0.);
+      }
+      _mesh->gXyz2gUvw(iFVi, gXyzV, gUvwV);
+      for (int l = 0; l < numJacNodes; l++) {
+        gradLambda[indGSJ(iEl, l, iPC)] = gUvwV[l][0];
+        if (_mesh->nPCFV(iFVi) >= 2) gradLambda[indGSJ(iEl, l, iPC+1)] = gUvwV[l][1];
+        if (_mesh->nPCFV(iFVi) == 3) gradLambda[indGSJ(iEl, l, iPC+2)] = gUvwV[l][2];
+      }
+      iPC += _mesh->nPCFV(iFVi);
+    }
+  }
+}
+
+
+#endif /* _OPTHOMOBJCONTRIBMETRICMIN_H_ */
diff --git a/contrib/HighOrderMeshOptimizer/OptHomObjContribScaledJac.h b/contrib/HighOrderMeshOptimizer/OptHomObjContribScaledJac.h
new file mode 100644
index 0000000000000000000000000000000000000000..6aa91d0b1810549911415ab3afa2c7d9a95d77db
--- /dev/null
+++ b/contrib/HighOrderMeshOptimizer/OptHomObjContribScaledJac.h
@@ -0,0 +1,219 @@
+// TODO: Copyright
+
+#ifndef _OPTHOMOBJCONTRIBSCALEDJAC_H_
+#define _OPTHOMOBJCONTRIBSCALEDJAC_H_
+
+#include "MeshOptPatch.h"
+#include "MeshOptObjContrib.h"
+
+
+template<class FuncType>
+class ObjContribScaledJac : public ObjContrib, public FuncType
+{
+public:
+  ObjContribScaledJac(double weight);
+  virtual ~ObjContribScaledJac() {}
+  virtual void initialize(Patch *mesh);
+  virtual bool fail() { return _min <= 0.; }
+  virtual bool addContrib(double &Obj, alglib::real_1d_array &gradObj);
+  virtual void updateParameters() { FuncType::updateParameters(_min, _max); }
+  virtual bool targetReached() { return FuncType::targetReached(_min, _max); }
+  virtual bool stagnated() { return FuncType::stagnated(_min, _max); }
+  virtual void updateMinMax();
+  virtual void updateResults(MeshOptResults &res) const;
+
+protected:
+  Patch *_mesh;
+  double _weight;
+  std::vector<int> _nBezEl;                                                             // Number of Bezier poly. and for an el.
+  std::vector<fullMatrix<double> > _scaledNormEl;                                       // Normals to 2D elements for Jacobian regularization and scaling
+  std::vector<double> _invStraightJac;                                                  // Initial Jacobians for 3D elements
+//  inline const int &nBezEl(int iEl) { return _nBezEl[iEl]; }
+  inline int indGSJ(int iEl, int l, int iPC) { return iPC*_nBezEl[iEl]+l; }
+  void scaledJacAndGradients(int iEl, std::vector<double> &sJ, std::vector<double> &gSJ);
+//  void calcScaledNormalEl2D(const std::map<MElement*,GEntity*> &element2entity, int iEl);
+  void calcScaledNormalEl2D(int iEl);
+};
+
+
+template<class FuncType>
+ObjContribScaledJac<FuncType>::ObjContribScaledJac(double weight) :
+  ObjContrib("ScaledJac", FuncType::getNamePrefix()+"ScaledJac"),
+  _mesh(0), _weight(weight)
+{
+}
+
+
+template<class FuncType>
+void ObjContribScaledJac<FuncType>::initialize(Patch *mesh)
+{
+  _mesh = mesh;
+
+  // Initialize _nBezEl
+  _nBezEl.resize(_mesh->nEl());
+  for (int iEl=0; iEl<_mesh->nEl(); iEl++)
+   _nBezEl[iEl] = _mesh->el(iEl)->getJacobianFuncSpace()->getNumJacNodes();
+
+  // Set normals to 2D elements (with magnitude of inverse Jacobian) or initial
+  // Jacobians of 3D elements
+  if (_mesh->dim() == 2) {
+    _scaledNormEl.resize(_mesh->nEl());
+//      for (int iEl = 0; iEl < nEl(); iEl++) calcScaledNormalEl2D(element2entity,iEl);
+    for (int iEl = 0; iEl < _mesh->nEl(); iEl++) calcScaledNormalEl2D(iEl);
+  }
+  else {
+    _invStraightJac.resize(_mesh->nEl());
+    double dumJac[3][3];
+    for (int iEl = 0; iEl < _mesh->nEl(); iEl++)
+      _invStraightJac[iEl] = 1. / fabs(_mesh->el(iEl)->getPrimaryJacobian(0.,0.,0.,dumJac));
+  }
+
+  updateMinMax();
+  FuncType::initialize(_min, _max);
+}
+
+
+template<class FuncType>
+bool ObjContribScaledJac<FuncType>::addContrib(double &Obj, alglib::real_1d_array &gradObj)
+{
+  _min = BIGVAL;
+  _max = -BIGVAL;
+
+  for (int iEl = 0; iEl < _mesh->nEl(); iEl++) {
+    std::vector<double> sJ(_nBezEl[iEl]);                                     // Scaled Jacobians
+    std::vector<double> gSJ(_nBezEl[iEl]*_mesh->nPCEl(iEl));                  // Gradients of scaled Jacobians
+    scaledJacAndGradients(iEl, sJ, gSJ);
+    for (int l = 0; l < _nBezEl[iEl]; l++) {                                  // Add contribution for each Bezier coeff.
+      Obj += _weight * FuncType::compute(sJ[l]);
+      const double dfact = _weight * FuncType::computeDiff(sJ[l]);
+      for (int iPC = 0; iPC < _mesh->nPCEl(iEl); iPC++)
+        gradObj[_mesh->indPCEl(iEl, iPC)] += dfact * gSJ[indGSJ(iEl, l, iPC)];
+      _min = std::min(_min, sJ[l]);
+      _max = std::max(_max, sJ[l]);
+    }
+  }
+
+  return true;
+}
+
+
+template<class FuncType>
+void ObjContribScaledJac<FuncType>::updateMinMax()
+{
+  _min = BIGVAL;
+  _max = -BIGVAL;
+
+  for (int iEl = 0; iEl < _mesh->nEl(); iEl++) {
+    std::vector<double> sJ(_nBezEl[iEl]);                         // Scaled Jacobians
+    std::vector<double> dumGSJ(_nBezEl[iEl]*_mesh->nPCEl(iEl));   // Gradients of scaled Jacobians
+    scaledJacAndGradients(iEl, sJ, dumGSJ);
+    for (int l = 0; l < _nBezEl[iEl]; l++) {                      // Check each Bezier coeff.
+      _min = std::min(_min, sJ[l]);
+      _max = std::max(_max, sJ[l]);
+    }
+  }
+}
+
+
+template<class FuncType>
+void ObjContribScaledJac<FuncType>::updateResults(MeshOptResults &res) const
+{
+  res.minScaledJac = std::min(_min, res.minScaledJac);
+  res.maxScaledJac = std::max(_max, res.maxScaledJac);
+}
+
+
+// TODO: Re-introduce normal to geometry?
+//void Mesh::calcScaledNormalEl2D(const std::map<MElement*,GEntity*> &element2entity, int iEl)
+template<class FuncType>
+void ObjContribScaledJac<FuncType>::calcScaledNormalEl2D(int iEl)
+{
+  const JacobianBasis *jac = _mesh->el(iEl)->getJacobianFuncSpace();
+
+  fullMatrix<double> primNodesXYZ(jac->getNumPrimMapNodes(), 3);
+//  SVector3 geoNorm(0.,0.,0.);
+//  std::map<MElement*,GEntity*>::const_iterator itEl2ent = element2entity.find(_el[iEl]);
+//  GEntity *ge = (itEl2ent == element2entity.end()) ? 0 : itEl2ent->second;
+//  const bool hasGeoNorm = ge && (ge->dim() == 2) && ge->haveParametrization();
+  for (int i=0; i<jac->getNumPrimMapNodes(); i++) {
+    const int &iV = _mesh->el2V(iEl, i);
+    primNodesXYZ(i,0) = _mesh->xyz(iV).x();
+    primNodesXYZ(i,1) = _mesh->xyz(iV).y();
+    primNodesXYZ(i,2) = _mesh->xyz(iV).z();
+//    if (hasGeoNorm && (_vert[iV]->onWhat() == ge)) {
+//      double u, v;
+//      _vert[iV]->getParameter(0,u);
+//      _vert[iV]->getParameter(1,v);
+//      geoNorm += ((GFace*)ge)->normal(SPoint2(u,v));
+//    }
+  }
+//  if (hasGeoNorm && (geoNorm.normSq() == 0.)) {
+//    SPoint2 param = ((GFace*)ge)->parFromPoint(_el[iEl]->barycenter(true),false);
+//    geoNorm = ((GFace*)ge)->normal(param);
+//  }
+
+  fullMatrix<double> &elNorm = _scaledNormEl[iEl];
+  elNorm.resize(1, 3);
+  const double norm = jac->getPrimNormal2D(primNodesXYZ, elNorm);
+  double factor = 1./norm;
+//  if (hasGeoNorm) {
+//    const double scal = geoNorm(0)*elNorm(0,0)+geoNorm(1)*elNorm(0,1)+geoNorm(2)*elNorm(0,2);
+//    if (scal < 0.) factor = -factor;
+//  }
+  elNorm.scale(factor);   // Re-scaling normal here is faster than an extra scaling operation on the Jacobian
+
+}
+
+
+template<class FuncType>
+void ObjContribScaledJac<FuncType>::scaledJacAndGradients(int iEl, std::vector<double> &sJ,
+                                                                   std::vector<double> &gSJ)
+{
+  const JacobianBasis *jacBasis = _mesh->el(iEl)->getJacobianFuncSpace();
+  const int &numJacNodes = _nBezEl[iEl];
+  const int &numMapNodes = _mesh->nNodEl(iEl);
+  fullMatrix<double> JDJ(numJacNodes, 3*numMapNodes+1), BDB(numJacNodes, 3*numMapNodes+1);
+
+  // Coordinates of nodes
+  fullMatrix<double> nodesXYZ(numMapNodes, 3), normals(_mesh->dim(), 3);
+  for (int i = 0; i < numMapNodes; i++) {
+    const int &iVi = _mesh->el2V(iEl, i);
+    nodesXYZ(i, 0) = _mesh->xyz(iVi).x();
+    nodesXYZ(i, 1) = _mesh->xyz(iVi).y();
+    nodesXYZ(i, 2) = _mesh->xyz(iVi).z();
+  }
+
+  // Calculate Jacobian and gradients, scale if 3D (already scaled by
+  // regularization normals in 2D)
+  jacBasis->getSignedJacAndGradients(nodesXYZ, _scaledNormEl[iEl],JDJ);
+  if (_mesh->dim() == 3) JDJ.scale(_invStraightJac[iEl]);
+
+  // Transform Jacobian and gradients from Lagrangian to Bezier basis
+  jacBasis->lag2Bez(JDJ,BDB);
+
+  // Scaled jacobian
+  for (int l = 0; l < numJacNodes; l++) sJ[l] = BDB (l, 3*numMapNodes);
+
+  // Gradients of the scaled jacobian
+  int iPC = 0;
+  std::vector<SPoint3> gXyzV(numJacNodes);
+  std::vector<SPoint3> gUvwV(numJacNodes);
+  for (int i = 0; i < numMapNodes; i++) {
+    const int &iFVi = _mesh->el2FV(iEl, i);
+    if (iFVi >= 0) {
+      for (int l = 0; l < numJacNodes; l++)
+        gXyzV[l] = SPoint3(BDB(l, i), BDB(l, i+numMapNodes), BDB(l, i+2*numMapNodes));
+      _mesh->gXyz2gUvw(iFVi, gXyzV, gUvwV);
+      for (int l = 0; l < numJacNodes; l++) {
+        gSJ[indGSJ(iEl, l, iPC)] = gUvwV[l][0];
+        if (_mesh->nPCFV(iFVi) >= 2) gSJ[indGSJ(iEl, l, iPC+1)] = gUvwV[l][1];
+        if (_mesh->nPCFV(iFVi) == 3) gSJ[indGSJ(iEl, l, iPC+2)] = gUvwV[l][2];
+      }
+      iPC += _mesh->nPCFV(iFVi);
+    }
+  }
+
+}
+
+
+#endif /* _OPTHOMOBJCONTRIBSCALEDJAC_H_ */
diff --git a/contrib/HighOrderMeshOptimizer/OptHomRun.cpp b/contrib/HighOrderMeshOptimizer/OptHomRun.cpp
index 5342856ebd5be2b1cc0d73c86d2bbda87da2fbf8..877beae09aef80f69f0649c9fd797343de6a2342 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomRun.cpp
+++ b/contrib/HighOrderMeshOptimizer/OptHomRun.cpp
@@ -737,3 +737,105 @@ void HighOrderMeshOptimizer(GModel *gm, OptHomParameters &p)
   Msg::Error("High-order mesh optimizer requires BFGS");
 #endif
 }
+
+
+//#include "GModel.h"
+#include "GEntity.h"
+//#include "MElement.h"
+//#include "OptHomRun.h"
+#include "MeshOptCommon.h"
+#include "MeshOptObjContribFunc.h"
+#include "MeshOptObjectiveFunction.h"
+#include "MeshOptObjContrib.h"
+#include "MeshOptObjContribScaledNodeDispSq.h"
+#include "OptHomObjContribScaledJac.h"
+#include "OptHomObjContribMetricMin.h"
+#include "MeshOptimizer.h"
+
+
+struct HOPatchParameters : public MeshOptParameters::PatchParameters
+{
+  HOPatchParameters(const OptHomParameters &p);
+  virtual ~HOPatchParameters() {}
+  virtual double elBadness(const MElement *el);
+  virtual double maxDistance(const MElement *el);
+private:
+  double jacMin, jacMax;
+  double distanceFactor;
+};
+
+
+HOPatchParameters::HOPatchParameters(const OptHomParameters &p)
+{
+  jacMin = p.BARRIER_MIN;
+  jacMax = (p.BARRIER_MAX > 0.) ? p.BARRIER_MAX : 1.e300;
+  strategy = (p.strategy == 1) ? MeshOptParameters::STRAT_ONEBYONE :
+                                        MeshOptParameters::STRAT_CONNECTED;
+  minLayers = (p.dim == 3) ? 1 : 0;
+  maxLayers = p.nbLayers;
+  distanceFactor = p.distanceFactor;
+  if (strategy == MeshOptParameters::STRAT_CONNECTED)
+    weakMerge = (p.strategy == 2);
+  else {
+    maxAdaptPatch = p.maxAdaptBlob;
+    maxLayersAdaptFact = p.adaptBlobLayerFact;
+    distanceAdaptFact = p.adaptBlobDistFact;
+  }
+}
+
+
+double HOPatchParameters::elBadness(const MElement *el) {
+  double jmin, jmax;
+  el->scaledJacRange(jmin, jmax);
+  return std::min(jmin-jacMin, 0.) + std::min(jacMax-jmax, 0.);
+}
+
+
+double HOPatchParameters::maxDistance(const MElement *el) {
+  return distanceFactor * el->maxDistToStraight();
+}
+
+
+void HighOrderMeshOptimizerNew(GModel *gm, OptHomParameters &p)
+{
+  Msg::StatusBar(true, "Optimizing high order mesh...");
+
+  MeshOptParameters par;
+  par.dim = p.dim;
+  par.onlyVisible = p.onlyVisible;
+  par.fixBndNodes = p.fixBndNodes;
+  HOPatchParameters patch(p);
+  par.patch = &patch;
+  par.optDisplay = 30;
+  par.verbose = 4;
+  ObjContribScaledNodeDispSq<ObjContribFuncSimple> nodeDistFunc(p.weightFixed, p.weightFree);
+  ObjContribScaledJac<ObjContribFuncBarrierMin> minJacBarFunc(1.);
+  ObjContribScaledJac<ObjContribFuncBarrierMinMax> minMaxJacBarFunc(1.);
+  MeshOptParameters::PassParameters minJacPass;
+  minJacPass.barrierIterMax = p.optPassMax;
+  minJacPass.optIterMax = p.itMax;
+  minJacPass.objFunc = ObjectiveFunction();
+  minJacPass.objFunc.push_back(&nodeDistFunc);
+  minJacBarFunc.setTarget(p.BARRIER_MIN, 1.);
+  minJacPass.objFunc.push_back(&minJacBarFunc);
+  par.pass.push_back(minJacPass);
+  if (p.BARRIER_MAX > 0.) {
+    MeshOptParameters::PassParameters minMaxJacPass;
+    minMaxJacPass.barrierIterMax = p.optPassMax;
+    minMaxJacPass.optIterMax = p.itMax;
+    minMaxJacPass.objFunc = ObjectiveFunction();
+    minMaxJacPass.objFunc.push_back(&nodeDistFunc);
+    minMaxJacBarFunc.setTarget(p.BARRIER_MAX, 1.);
+    minMaxJacPass.objFunc.push_back(&minMaxJacBarFunc);
+    par.pass.push_back(minMaxJacPass);
+  }
+  MeshOptResults res;
+
+  meshOptimizer(gm, par, res);
+
+  p.CPU = res.CPU;
+  p.minJac = res.minScaledJac;
+  p.maxJac = res.maxScaledJac;
+
+  Msg::StatusBar(true, "Done optimizing high order mesh (%g s)", p.CPU);
+}
diff --git a/contrib/HighOrderMeshOptimizer/OptHomRun.h b/contrib/HighOrderMeshOptimizer/OptHomRun.h
index f1d172f693c1e334609b34d1ced78b2f76d143fb..cceed2f94c2f5c84ce98740e58ef6fab928ce72f 100644
--- a/contrib/HighOrderMeshOptimizer/OptHomRun.h
+++ b/contrib/HighOrderMeshOptimizer/OptHomRun.h
@@ -68,13 +68,14 @@ struct OptHomParameters {
     : BARRIER_MIN_METRIC(-1.), BARRIER_MIN(0.1), BARRIER_MAX(2.0), weightFixed(1000.),
       weightFree (1.), nbLayers (6) , dim(3) , itMax(300), onlyVisible(true),
       distanceFactor(12), fixBndNodes(false), strategy(0), maxAdaptBlob(3),
-      adaptBlobLayerFact(2.), adaptBlobDistFact(2.), optPrimSurfMesh(false),optCAD(false),
-      optCADWeight(1000.),optCADDistMax(1.e22),discrTolerance(1.e-4)
+      adaptBlobLayerFact(2.), adaptBlobDistFact(2.), optPrimSurfMesh(false), optCAD(false),
+      optCADWeight(1000.), optCADDistMax(1.e22), discrTolerance(1.e-4)
   {
   }
 };
 
 void HighOrderMeshOptimizer(GModel *gm, OptHomParameters &p);
+void HighOrderMeshOptimizerNew(GModel *gm, OptHomParameters &p);
 // distanceDefinition 1) Hausdorff 2) Area/Length 3) Frechet (not done)
 double ComputeDistanceToGeometry (GEntity *ge , int distanceDefinition,double tolerance) ;
 
diff --git a/contrib/MeshOptimizer/CMakeLists.txt b/contrib/MeshOptimizer/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..cc41baa392172c8a44fad4815f7c3528158d12a1
--- /dev/null
+++ b/contrib/MeshOptimizer/CMakeLists.txt
@@ -0,0 +1,18 @@
+# Gmsh - Copyright (C) 1997-2014 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@geuz.org>.
+
+set(SRC
+  MeshOpt.cpp
+  MeshOptimizer.cpp
+  MeshOptPatch.cpp
+  MeshOptObjectiveFunction.cpp
+  MeshOptObjContrib.cpp
+  MeshOptObjContribFunc.cpp
+  MeshOptVertexCoord.cpp
+  MeshOptCommon.cpp 
+)
+
+file(GLOB_RECURSE HDR RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} *.hpp)
+append_gmsh_src(contrib/MeshOptimizer "${SRC};${HDR}")
diff --git a/contrib/MeshOptimizer/MeshOpt.cpp b/contrib/MeshOptimizer/MeshOpt.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f3812c0a81918a4d6956baf2f98da2c34a76c447
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOpt.cpp
@@ -0,0 +1,239 @@
+// Copyright (C) 2013 ULg-UCL
+//
+// Permission is hereby granted, free of charge, to any person
+// obtaining a copy of this software and associated documentation
+// files (the "Software"), to deal in the Software without
+// restriction, including without limitation the rights to use, copy,
+// modify, merge, publish, distribute, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished
+// to do so, provided that the above copyright notice(s) and this
+// permission notice appear in all copies of the Software and that
+// both the above copyright notice(s) and this permission notice
+// appear in supporting documentation.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
+// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
+// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
+// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+// OF THIS SOFTWARE.
+//
+// Please report all bugs and problems to the public mailing list
+// <gmsh@geuz.org>.
+//
+// Contributors: Thomas Toulorge, Jonathan Lambrechts
+
+#include <iostream>
+#include <algorithm>
+#include "GmshMessage.h"
+#include "GmshConfig.h"
+#include "MeshOptObjContrib.h"
+#include "MeshOptObjectiveFunction.h"
+#include "MeshOptCommon.h"
+#include "MeshOpt.h"
+
+#if defined(HAVE_BFGS)
+
+#include "ap.h"
+#include "alglibinternal.h"
+#include "alglibmisc.h"
+#include "linalg.h"
+#include "optimization.h"
+
+
+namespace {
+
+
+void evalObjGradFunc(const alglib::real_1d_array &x, double &Obj,
+                     alglib::real_1d_array &gradObj, void *MOInst)
+{
+  (static_cast<MeshOpt*>(MOInst))->evalObjGrad(x, Obj, gradObj);
+}
+
+
+void printProgressFunc(const alglib::real_1d_array &x, double Obj, void *MOInst)
+{
+  (static_cast<MeshOpt*>(MOInst))->printProgress(x,Obj);
+}
+
+
+}
+
+
+MeshOpt::MeshOpt(const std::map<MElement*,GEntity*> &element2entity,
+                 const std::set<MElement*> &els, std::set<MVertex*> & toFix,
+                 bool fixBndNodes) :
+  patch(element2entity, els, toFix, fixBndNodes), _verbose(0),
+  _iPass(0), _objFunc(0), _iter(0), _intervDisplay(0), _initObj(0)
+{
+}
+
+
+void MeshOpt::evalObjGrad(const alglib::real_1d_array &x, double &obj,
+                                         alglib::real_1d_array &gradObj)
+{
+  patch.updateMesh(x.getcontent());
+  _objFunc->compute(obj, gradObj);
+  if (_objFunc->targetReached()) {
+    if (_verbose > 2) Msg::Info("Reached target values, setting null gradient");
+    obj = 0.;
+    for (int i = 0; i < gradObj.length(); i++) gradObj[i] = 0.;
+  }
+}
+
+
+void MeshOpt::printProgress(const alglib::real_1d_array &x, double Obj)
+{
+  _iter++;
+
+  if ((_verbose > 2) && (_iter % _intervDisplay == 0))
+    Msg::Info(("--> Iteration %3d --- OBJ %12.5E (relative decrease = %12.5E)" +
+              _objFunc->minMaxStr()).c_str(), _iter, Obj, Obj/_initObj);
+}
+
+
+void MeshOpt::calcScale(alglib::real_1d_array &scale)
+{
+  scale.setlength(patch.nPC());
+
+  // Calculate scale
+  for (int iFV = 0; iFV < patch.nFV(); iFV++) {
+    std::vector<double> scaleFV(patch.nPCFV(iFV),1.);
+    patch.pcScale(iFV,scaleFV);
+    for (int iPC = 0; iPC < patch.nPCFV(iFV); iPC++)
+      scale[patch.indPCFV(iFV,iPC)] = scaleFV[iPC];
+  }
+}
+
+
+void MeshOpt::updateResults(MeshOptResults &res)
+{
+  _objFunc->updateResults(res);
+}
+
+
+void MeshOpt::runOptim(alglib::real_1d_array &x,
+                        const alglib::real_1d_array &initGradObj, int itMax)
+{
+  static const double EPSG = 0.;
+  static const double EPSF = 0.;
+  static const double EPSX = 0.;
+
+  _iter = 0;
+
+  alglib::real_1d_array scale;
+  calcScale(scale);
+
+  int iterationscount = 0, nfev = 0, terminationtype = -1;
+  alglib::mincgstate state;
+  alglib::mincgreport rep;
+  try {
+    mincgcreate(x, state);
+    mincgsetscale(state,scale);
+    mincgsetprecscale(state);
+    mincgsetcond(state, EPSG, EPSF, EPSX, itMax);
+    mincgsetxrep(state, true);
+    alglib::mincgoptimize(state, evalObjGradFunc, printProgressFunc, this);
+    mincgresults(state, x, rep);
+  }
+  catch(alglib::ap_error &e) {
+    Msg::Error("%s", e.msg.c_str());
+  }
+  iterationscount = rep.iterationscount;
+  nfev = rep.nfev;
+  terminationtype = rep.terminationtype;
+
+  if (_verbose > 2) {
+    Msg::Info("Optimization finalized after %d iterations (%d function evaluations),",
+              iterationscount, nfev);
+    switch(int(terminationtype)) {
+    case 1: Msg::Info("because relative function improvement is no more than EpsF"); break;
+    case 2: Msg::Info("because relative step is no more than EpsX"); break;
+    case 4: Msg::Info("because gradient norm is no more than EpsG"); break;
+    case 5: Msg::Info("because the maximum number of steps was taken"); break;
+    default: Msg::Info("with code %d", int(terminationtype)); break;
+    }
+  }
+}
+
+
+int MeshOpt::optimize(MeshOptParameters &par)
+{
+  _intervDisplay = par.optDisplay;
+  _verbose = par.verbose;
+
+  // Set initial guess & result
+  int result = 1;
+  alglib::real_1d_array x;
+  x.setlength(patch.nPC());
+  patch.getUvw(x.getcontent());
+
+  if (_verbose > 2)
+    Msg::Info("Patch composed of %i elements, %i vertices, %i free vertices, %i variables",
+                                          patch.nEl(), patch.nVert(), patch.nFV(), patch.nPC());
+
+  // Loop on passes
+  for (_iPass=0; _iPass<par.pass.size(); _iPass++) {
+
+    // Set objective function Output if required
+    _objFunc = &par.pass[_iPass].objFunc;
+    if (_verbose > 2) {
+      std::string msgStr = "* Pass %d with contributions: " + _objFunc->contribNames();
+      Msg::Info(msgStr.c_str(), _iPass);
+    }
+
+    // Initialize contributions
+    _objFunc->initialize(&patch);
+    _objFunc->updateParameters();
+
+    // Calculate initial objective function value and gradient
+   _initObj = 0.;
+    alglib::real_1d_array gradObj;
+    gradObj.setlength(patch.nPC());
+    for (int i = 0; i < patch.nPC(); i++) gradObj[i] = 0.;
+    evalObjGrad(x, _initObj, gradObj);
+
+    // Loop for update of objective function parameters (barrier movement)
+    bool targetReached = _objFunc->targetReached();
+    for (int iBar=0; (iBar<par.pass[_iPass].barrierIterMax) && (!targetReached); iBar++) {
+      if (_verbose > 2) Msg::Info("--- Optimization run %d", iBar);
+      _objFunc->updateParameters();
+      runOptim(x, gradObj, par.pass[_iPass].optIterMax);
+      _objFunc->updateMinMax();
+      targetReached = _objFunc->targetReached();
+      if (_objFunc->stagnated()) {
+        if (_verbose > 2) Msg::Info("Stagnation detected, stopping optimization");
+        break;
+      }
+    }
+
+    // Check results of pass and output if required
+    if (!targetReached) result = 0;
+    std::string failMeasures = _objFunc->failMeasures();
+    if (!failMeasures.empty()) {
+      result = -1;
+      if (_verbose > 2)
+        Msg::Error("Failed to reach critical value in pass %d "
+                    "for measure(s): %s", _iPass, failMeasures.c_str());
+    }
+    if (_verbose > 2) {
+      if (targetReached)
+        Msg::Info("Target reached for pass %d", _iPass);
+      else {
+        std::string failedTargets = _objFunc->targetsNotReached();
+        Msg::Warning("Failed to reach target in pass %d for "
+                      "contributions %s", _iPass, failedTargets.c_str());
+      }
+    }
+    if (result == -1) break;
+  }                                                                       // Pass loop
+
+  return result;
+}
+
+
+#endif
diff --git a/contrib/MeshOptimizer/MeshOpt.h b/contrib/MeshOptimizer/MeshOpt.h
new file mode 100644
index 0000000000000000000000000000000000000000..6d72d1b30205184cd1255ea7e640ed0b6b6f01de
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOpt.h
@@ -0,0 +1,73 @@
+// Copyright (C) 2013 ULg-UCL
+//
+// Permission is hereby granted, free of charge, to any person
+// obtaining a copy of this software and associated documentation
+// files (the "Software"), to deal in the Software without
+// restriction, including without limitation the rights to use, copy,
+// modify, merge, publish, distribute, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished
+// to do so, provided that the above copyright notice(s) and this
+// permission notice appear in all copies of the Software and that
+// both the above copyright notice(s) and this permission notice
+// appear in supporting documentation.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
+// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
+// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
+// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+// OF THIS SOFTWARE.
+//
+// Please report all bugs and problems to the public mailing list
+// <gmsh@geuz.org>.
+//
+// Contributors: Thomas Toulorge, Jonathan Lambrechts
+
+#ifndef _MESHOPT_H_
+#define _MESHOPT_H_
+
+#include <string>
+#include <math.h>
+#include "MeshOptObjectiveFunction.h"
+#include "MeshOptPatch.h"
+
+#if defined(HAVE_BFGS)
+
+#include "ap.h"
+
+
+class MeshOptParameters;
+
+
+class MeshOpt
+{
+public:
+  Patch patch;
+  MeshOpt(const std::map<MElement*,GEntity*> &element2entity,
+          const std::set<MElement*> &els, std::set<MVertex*> &toFix, bool fixBndNodes);
+  int optimize(MeshOptParameters &par);
+  void updateMesh(const alglib::real_1d_array &x);
+  void updateResults(MeshOptResults &res);
+  void evalObjGrad(const alglib::real_1d_array &x,
+                    double &Obj, alglib::real_1d_array &gradObj);
+  void printProgress(const alglib::real_1d_array &x, double Obj);
+
+ private:
+  int _verbose;
+  int _iPass;
+  ObjectiveFunction *_objFunc;                                                      // Current contributions to objective function
+  int _iter, _intervDisplay;                                                        // Current iteration, interval of iterations for reporting
+  double _initObj;                                                                  // Values for reporting
+  void calcScale(alglib::real_1d_array &scale);
+  void runOptim(alglib::real_1d_array &x,
+                const alglib::real_1d_array &initGradObj, int itMax);
+};
+
+
+#endif
+
+#endif
diff --git a/contrib/MeshOptimizer/MeshOptCommon.cpp b/contrib/MeshOptimizer/MeshOptCommon.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b1d2391a77f0a23f4cae1fbb8e94969827e5788e
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptCommon.cpp
@@ -0,0 +1,11 @@
+// TODO: Copyright
+
+#include "MeshOptCommon.h"
+
+
+MeshOptResults::MeshOptResults() :
+  success(-1), CPU(0.), minNodeDisp(BIGVAL), maxNodeDisp(-BIGVAL),
+  minScaledJac(BIGVAL), maxScaledJac(-BIGVAL), minMetricMin(BIGVAL),
+  maxMetricMin(-BIGVAL)
+{
+}
diff --git a/contrib/MeshOptimizer/MeshOptCommon.h b/contrib/MeshOptimizer/MeshOptCommon.h
new file mode 100644
index 0000000000000000000000000000000000000000..43137280d045b04369f936846fd940ed2346c4b8
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptCommon.h
@@ -0,0 +1,83 @@
+// Copyright (C) 2014 ULg-UCL
+//
+// Permission is hereby granted, free of charge, to any person
+// obtaining a copy of this software and associated documentation
+// files (the "Software"), to deal in the Software without
+// restriction, including without limitation the rights to use, copy,
+// modify, merge, publish, distribute, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished
+// to do so, provided that the above copyright notice(s) and this
+// permission notice appear in all copies of the Software and that
+// both the above copyright notice(s) and this permission notice
+// appear in supporting documentation.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
+// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
+// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
+// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+// OF THIS SOFTWARE.
+//
+// Please report all bugs and problems to the public mailing list
+// <gmsh@geuz.org>.
+//
+// Contributors: Thomas Toulorge, Jonathan Lambrechts
+
+#ifndef _MESHOPTCOMMON_H_
+#define _MESHOPTCOMMON_H_
+
+#include <vector>
+#include "MeshOptObjectiveFunction.h"
+
+
+class MElement;
+
+
+struct MeshOptResults {                             // Output of mesh optimization
+  int success;                                      // Success flag: -1 = fail, 0 = partial fail (target not reached), 1 = success
+  double CPU;                                       // Time for optimization
+  double minNodeDisp, maxNodeDisp;                  // Range of node displacement
+  double minScaledJac, maxScaledJac;                // Range of Scaled Jacobians
+  double minMetricMin, maxMetricMin;                // Range of min. of metric
+  MeshOptResults();
+private:
+  static const double BIGVAL = 1.e300;
+};
+
+
+struct MeshOptParameters {                             // Parameters controlling the strategy
+  enum { STRAT_CONNECTED, STRAT_ONEBYONE };
+  struct PassParameters {                              // Parameters controlling the optimization procedure in each pass
+    ObjectiveFunction objFunc;                         // Contributions to objective function
+    int optIterMax ;                                   // Max. number of opt. iterations each time the barrier is moved
+    int barrierIterMax ;                               // Max. number of times the barrier is moved
+  };
+  struct PatchParameters {
+    int strategy;                                      // Strategy: connected patches or adaptive one-by-one
+    int minLayers, maxLayers;                          // Min. and max. nb. of layers around a bad element in patch
+    union {
+      struct {                                         // If adaptive strategy:
+        int maxAdaptPatch;                             // Max. nb. of adaptation iterations
+        int maxLayersAdaptFact;                        // Growth rate in number of layers around a bad element
+        double distanceAdaptFact;                      // Growth rate in max. distance from bad element
+      };
+      bool weakMerge;                                   // If connected strategy: weak or strong merging of patches
+    };
+    virtual double elBadness(const MElement *el) = 0;   // Pointer to function returning "badness" of element (for patch creation)
+    virtual double maxDistance(const MElement *el) = 0; // Pointer to function checking the patch distance criterion for a given bad element
+  };
+  int dim ;                                             // Which dimension to optimize
+  bool onlyVisible ;                                    // Apply optimization to visible entities ONLY
+  bool fixBndNodes;                                     // If points can move on boundaries
+  PatchParameters *patch;
+  std::vector<PassParameters> pass;
+  int optDisplay;                                       // Sampling rate in opt. iterations for display
+  int verbose;                                          // Level of information displayed and written to disk
+};
+
+
+#endif
diff --git a/contrib/MeshOptimizer/MeshOptObjContrib.cpp b/contrib/MeshOptimizer/MeshOptObjContrib.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c09fb80ad76fa0a7c9c92125986af36e93e49b5
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptObjContrib.cpp
@@ -0,0 +1,9 @@
+// TODO: Copyright
+
+#include "MeshOptObjContrib.h"
+
+
+ObjContrib::ObjContrib(std::string mesName, std::string name) :
+ _min(0.), _max(0.), _measureName(mesName), _name(name)
+{
+}
diff --git a/contrib/MeshOptimizer/MeshOptObjContrib.h b/contrib/MeshOptimizer/MeshOptObjContrib.h
new file mode 100644
index 0000000000000000000000000000000000000000..7305cde4fd4538613008af1de8a8d12ede49f269
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptObjContrib.h
@@ -0,0 +1,39 @@
+// TODO: Copyright
+
+#ifndef _MESHOPTOBJCONTRIB_H_
+#define _MESHOPTOBJCONTRIB_H_
+
+#include <string>
+#include "ap.h"
+#include "MeshOptCommon.h"
+
+
+class Patch;
+
+
+class ObjContrib
+{
+public:
+  ObjContrib(std::string mesName, std::string name);
+  virtual ~ObjContrib() {}
+  const double getMin() { return _min; }
+  const double getMax() { return _max; }
+  const std::string &getName() const { return _name; }
+  const std::string &getMeasureName() const { return _measureName; }
+  virtual void initialize(Patch *mesh) = 0;
+  virtual bool fail() = 0;
+  virtual bool addContrib(double &Obj, alglib::real_1d_array &gradObj) = 0;
+  virtual void updateParameters() = 0;
+  virtual bool targetReached() = 0;
+  virtual bool stagnated() = 0;
+  virtual void updateMinMax() = 0;
+  virtual void updateResults(MeshOptResults &res) const = 0;
+
+protected:
+  static const double BIGVAL = 1.e300;
+  std::string _measureName, _name;
+  double _min, _max;
+};
+
+
+#endif /* _MESHOPTOBJCONTRIB_H_ */
diff --git a/contrib/MeshOptimizer/MeshOptObjContribFunc.cpp b/contrib/MeshOptimizer/MeshOptObjContribFunc.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fc6d407009d9753cf883942ec5b5df4c3099a336
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptObjContribFunc.cpp
@@ -0,0 +1,56 @@
+// TODO: Copyright
+
+#include <cmath>
+#include "MeshOptObjContribFunc.h"
+
+
+namespace {
+
+
+}
+
+
+ObjContribFuncBarrier::ObjContribFuncBarrier() :
+    _target(0.), _barrier(0.), _init(0.), _opt(0.)
+{
+}
+
+
+void ObjContribFuncBarrier::setTarget(double target, double opt)
+{
+  _target = target;
+  _opt = opt;
+}
+
+
+void ObjContribFuncBarrierMin::updateParameters(double vMin, double vMax)
+{
+  _init = vMin;
+  _barrier = vMin > 0. ? LOWMARGINMULT*vMin : UPMARGINMULT*vMin;
+}
+
+
+bool ObjContribFuncBarrierMin::stagnated(double vMin, double vMax)
+{
+  return (fabs((vMin-_init)/_init) < STAGTHRESHOLD);
+}
+
+
+void ObjContribFuncBarrierMax::updateParameters(double vMin, double vMax)
+{
+  _init = vMax;
+  _barrier = vMax > 0. ? UPMARGINMULT*vMax : LOWMARGINMULT*vMax;
+}
+
+
+bool ObjContribFuncBarrierMax::stagnated(double vMin, double vMax)
+{
+  return (fabs((vMax-_init)/_init) < STAGTHRESHOLD);
+}
+
+
+void ObjContribFuncBarrierMinMax::initialize(double vMin, double vMax)
+{
+  ObjContribFuncBarrierMax::initialize(vMin, vMax);
+  _fixedMinBarrier = vMin > 0. ? LOWMARGINMULT*vMin : UPMARGINMULT*vMin;
+}
diff --git a/contrib/MeshOptimizer/MeshOptObjContribFunc.h b/contrib/MeshOptimizer/MeshOptObjContribFunc.h
new file mode 100644
index 0000000000000000000000000000000000000000..2348c2ee80880064a23c131040e707cad0ff9d80
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptObjContribFunc.h
@@ -0,0 +1,147 @@
+// TODO: Copyright
+
+#ifndef _MESHOPTOBJCONTRIBFUNC_H_
+#define _MESHOPTOBJCONTRIBFUNC_H_
+
+#include <string>
+
+
+class ObjContribFuncSimple
+{
+protected:
+  std::string getNamePrefix() { return ""; }
+  void initialize(double vMin, double vMax) {}
+  void updateParameters(double vMin, double vMax) {}
+  bool targetReached(double vMin, double vMax) { return true; }
+  bool stagnated(double vMin, double vMax) { return false; };
+  double compute(double v) { return v; }
+  double computeDiff(double v) { return 1.; }
+};
+
+
+class ObjContribFuncBarrier
+{
+public:
+  ObjContribFuncBarrier();
+  void setTarget(double target, double opt=1.);
+
+protected:
+  static const double BLOWUPVAL = 1.e300, DIFFBLOWUPVAL = 1.e300;                 // Big values to set when function should be infinite
+  static const double LOWMARGINMULT = 0.9, UPMARGINMULT = 1.1;                    // Upper and lower margins w.r.t. min. & max. to set barrier
+  static const double STAGTHRESHOLD = 0.01;                                       // Threshold to consider that measures stagnates
+  double _opt;                                                                    // Optimal value of measure in barrier function
+  double _barrier, _target, _init;                                                // Current barrier, target and initial values of min./max. of measure
+  static double logBarrier(double v, double barrier, double opt);
+  static double diffLogBarrier(double v, double barrier, double opt);
+};
+
+
+class ObjContribFuncBarrierMin : public ObjContribFuncBarrier
+{
+protected:
+  std::string getNamePrefix() { return "BarrierMin"; }
+  void initialize(double vMin, double vMax) {}
+  void updateParameters(double vMin, double vMax);
+  bool targetReached(double vMin, double vMax) { return (vMin >= _target); }
+  bool stagnated(double vMin, double vMax);
+  double compute(double v);
+  double computeDiff(double v);
+};
+
+
+class ObjContribFuncBarrierMax : public ObjContribFuncBarrier
+{
+protected:
+  std::string getNamePrefix() { return "BarrierMax"; }
+  void initialize(double vMin, double vMax) {}
+  void updateParameters(double vMin, double vMax);
+  bool targetReached(double vMin, double vMax) { return (vMax <= _target); }
+  bool stagnated(double vMin, double vMax);
+  double compute(double v);
+  double computeDiff(double v);
+};
+
+
+class ObjContribFuncBarrierMinMax : public ObjContribFuncBarrierMax
+{
+protected:
+  std::string getNamePrefix() { return "BarrierMinMax"; }
+  void initialize(double vMin, double vMax);
+  inline double compute(double v);
+  inline double computeDiff(double v);
+
+protected:
+  double _fixedMinBarrier;
+};
+
+
+inline double ObjContribFuncBarrier::logBarrier(double v, double barrier, double opt)
+{
+  const double l = log((v-barrier) / (opt-barrier));
+  const double m = (v - opt);
+  return l*l + m*m;
+}
+
+
+inline double ObjContribFuncBarrier::diffLogBarrier(double v, double barrier, double opt)
+{
+  return 2. * ((v-opt) + log((v-barrier)/(opt-barrier))/(v-barrier));
+}
+
+
+inline double ObjContribFuncBarrierMin::compute(double v)
+{
+  if (v > _barrier) return logBarrier(v, _barrier, _opt);
+  else return BLOWUPVAL;
+}
+
+
+inline double ObjContribFuncBarrierMin::computeDiff(double v)
+{
+  if (v > _barrier) return diffLogBarrier(v, _barrier, _opt);
+  else return -DIFFBLOWUPVAL;
+}
+
+
+inline double ObjContribFuncBarrierMax::compute(double v)
+{
+  if (v < _barrier) return logBarrier(v, _barrier, _opt);
+  else return BLOWUPVAL;
+}
+
+
+inline double ObjContribFuncBarrierMax::computeDiff(double v)
+{
+  if (v < _barrier) return diffLogBarrier(v, _barrier, _opt);
+  else return DIFFBLOWUPVAL;
+}
+
+
+inline double ObjContribFuncBarrierMinMax::compute(double v)
+{
+  double obj;
+  if (v < _barrier) obj = logBarrier(v, _barrier, _opt);
+  else return BLOWUPVAL;
+  if (v > _fixedMinBarrier) {
+    obj += logBarrier(v, _fixedMinBarrier, _opt);
+    return obj;
+  }
+  else return BLOWUPVAL;
+}
+
+
+inline double ObjContribFuncBarrierMinMax::computeDiff(double v)
+{
+  double dobj;
+  if (v < _barrier) dobj = diffLogBarrier(v, _barrier, _opt);
+  else return DIFFBLOWUPVAL;
+  if (v > _fixedMinBarrier) {
+    dobj += diffLogBarrier(v, _fixedMinBarrier, _opt);
+    return dobj;
+  }
+  else return -DIFFBLOWUPVAL;
+}
+
+
+
+#endif /* _MESHOPTOBJCONTRIBFUNC_H_ */
diff --git a/contrib/MeshOptimizer/MeshOptObjContribScaledNodeDispSq.h b/contrib/MeshOptimizer/MeshOptObjContribScaledNodeDispSq.h
new file mode 100644
index 0000000000000000000000000000000000000000..b1829bb7ae0671e6929103b71f2c4a694d9cc631
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptObjContribScaledNodeDispSq.h
@@ -0,0 +1,139 @@
+// TODO: Copyright
+
+#ifndef _MESHOPTOBJCONTRIBSCALEDNODEDISPSQ_H_
+#define _MESHOPTOBJCONTRIBSCALEDNODEDISPSQ_H_
+
+#include "MeshOptPatch.h"
+#include "MeshOptObjContrib.h"
+
+
+template<class FuncType>
+class ObjContribScaledNodeDispSq : public ObjContrib, public FuncType
+{
+public:
+  ObjContribScaledNodeDispSq(double weightFixed, double weightFree);
+  virtual ~ObjContribScaledNodeDispSq() {}
+  virtual void initialize(Patch *mesh);
+  virtual bool fail() { return false; }
+  virtual bool addContrib(double &Obj, alglib::real_1d_array &gradObj);
+  virtual void updateParameters() { FuncType::updateParameters(_min, _max); }
+  virtual bool targetReached() { return FuncType::targetReached(_min, _max); }
+  virtual bool stagnated() { return FuncType::stagnated(_min, _max); }
+  virtual void updateMinMax();
+  virtual void updateResults(MeshOptResults &res) const;
+
+protected:
+  Patch *_mesh;
+  double _weightFixed, _weightFree;
+  double _invLengthScaleSq;                         // Square inverse of a length for node displacement scaling
+  inline double scaledNodeDispSq(int iFV);
+  inline void gradScaledNodeDispSq(int iFV, std::vector<double> &gDSq);
+
+};
+
+
+template<class FuncType>
+ObjContribScaledNodeDispSq<FuncType>::ObjContribScaledNodeDispSq(double weightFixed,
+                                                                    double weightFree) :
+  ObjContrib("ScaledNodeDispSq", FuncType::getNamePrefix()+"ScaledNodeDispSq"),
+  _mesh(0), _weightFixed(weightFixed), _weightFree(weightFree), _invLengthScaleSq(0.)
+{
+}
+
+
+template<class FuncType>
+void ObjContribScaledNodeDispSq<FuncType>::initialize(Patch *mesh)
+{
+  _mesh = mesh;
+
+  double maxDSq = 0.;
+  for (int iEl = 0; iEl < _mesh->nEl(); iEl++) {
+    const double d = _mesh->el(iEl)->maxDistToStraight(), dd = d*d;
+    if (dd > maxDSq) maxDSq = dd;
+  }
+  if (maxDSq < 1.e-10) {
+    double maxSSq = 0.;
+    for (int iEl = 0; iEl < _mesh->nEl(); iEl++) {
+      const double s = _mesh->el(iEl)->getOuterRadius(), ss = s*s;
+      if (ss > maxSSq) maxSSq = ss;
+    }
+    _invLengthScaleSq = 1./maxSSq;
+  }
+  else _invLengthScaleSq = 1./maxDSq;
+
+  updateMinMax();
+  FuncType::initialize(_min, _max);
+}
+
+
+template<class FuncType>
+bool ObjContribScaledNodeDispSq<FuncType>::addContrib(double &Obj,
+                                                      alglib::real_1d_array &gradObj)
+{
+  _min = BIGVAL;
+  _max = -BIGVAL;
+
+  for (int iFV = 0; iFV < _mesh->nFV(); iFV++) {
+    const double Factor = _mesh->forced(iFV) ? _weightFixed : _weightFree;
+    const double dSq = scaledNodeDispSq(iFV);
+    Obj += Factor * FuncType::compute(dSq);
+    std::vector<double> gDSq(_mesh->nPCFV(iFV));
+    gradScaledNodeDispSq(iFV, gDSq);
+    const double dfact = Factor * FuncType::computeDiff(dSq);
+    for (int iPC = 0; iPC < _mesh->nPCFV(iFV); iPC++)
+      gradObj[_mesh->indPCFV(iFV, iPC)] += dfact * gDSq[iPC];
+    _min = std::min(_min, dSq);
+    _max = std::max(_max, dSq);
+  }
+
+  return true;
+}
+
+
+template<class FuncType>
+void ObjContribScaledNodeDispSq<FuncType>::updateMinMax()
+{
+  _min = BIGVAL;
+  _max = -BIGVAL;
+
+  for (int iFV = 0; iFV < _mesh->nFV(); iFV++) {
+    const double dSq = scaledNodeDispSq(iFV);
+    _min = std::min(_min, dSq);
+    _max = std::max(_max, dSq);
+  }
+}
+
+
+template<class FuncType>
+void ObjContribScaledNodeDispSq<FuncType>::updateResults(MeshOptResults &res) const
+{
+  const double scaleSq = 1./_invLengthScaleSq;
+  res.minNodeDisp = std::min(sqrt(_min*scaleSq), res.minNodeDisp);
+  res.maxNodeDisp = std::max(sqrt(_max*scaleSq), res.maxNodeDisp);
+}
+
+
+template<class FuncType>
+double ObjContribScaledNodeDispSq<FuncType>::scaledNodeDispSq(int iFV)
+{
+  const int &iV = _mesh->fv2V(iFV);
+  const SPoint3 d = _mesh->xyz(iV)-_mesh->ixyz(iV);
+  return (d[0]*d[0]+d[1]*d[1]+d[2]*d[2])*_invLengthScaleSq;
+}
+
+
+template<class FuncType>
+void ObjContribScaledNodeDispSq<FuncType>::gradScaledNodeDispSq(int iFV, std::vector<double> &gDSq)
+{
+  const int &iV = _mesh->fv2V(iFV);
+  const SPoint3 gXyz = (_mesh->xyz(iV)-_mesh->ixyz(iV))*2.*_invLengthScaleSq;
+  SPoint3 gUvw;
+  _mesh->gXyz2gUvw(iFV, gXyz, gUvw);
+
+  gDSq[0] = gUvw[0];
+  if (_mesh->nPCFV(iFV) >= 2) gDSq[1] = gUvw[1];
+  if (_mesh->nPCFV(iFV) == 3) gDSq[2] = gUvw[2];
+}
+
+
+#endif /* _MESHOPTOBJCONTRIBSCALEDNODEDISPSQ_H_ */
diff --git a/contrib/MeshOptimizer/MeshOptObjectiveFunction.cpp b/contrib/MeshOptimizer/MeshOptObjectiveFunction.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3fac82abecf625c5614ae8b3467c70941efdeb4e
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptObjectiveFunction.cpp
@@ -0,0 +1,108 @@
+// TODO: Copyright
+
+#include <sstream>
+#include "MeshOptObjContrib.h"
+#include "MeshOptObjectiveFunction.h"
+
+
+void ObjectiveFunction::initialize(Patch *mesh)
+{
+   for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++)
+     (*it)->initialize(mesh);
+}
+
+
+std::string ObjectiveFunction::contribNames()
+{
+  std::vector<ObjContrib*>::iterator it=begin();
+  std::string str = (*it)->getName();
+  for (it++; it!=end(); it++) str += " " + (*it)->getName();
+  return str;
+}
+
+
+std::string ObjectiveFunction::minMaxStr()
+{
+  std::string str;
+  for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++) {
+    std::ostringstream oss;
+    oss << " -- Min. " + (*it)->getMeasureName() + " = " << (*it)->getMin();
+    oss << " -- Max. " + (*it)->getMeasureName() + " = " << (*it)->getMax();
+    str += oss.str();
+  }
+  return str;
+}
+
+
+void ObjectiveFunction::updateMinMax()
+{
+  for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++)
+    (*it)->updateMinMax();
+}
+
+
+void ObjectiveFunction::updateParameters()
+{
+  for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++)
+    (*it)->updateParameters();
+}
+
+
+void ObjectiveFunction::updateResults(MeshOptResults &res)
+{
+  for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++)
+    (*it)->updateResults(res);
+}
+
+
+bool ObjectiveFunction::stagnated()
+{
+  bool stagnated = false;
+  for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++)
+    stagnated |= (*it)->stagnated();
+  return stagnated;
+}
+
+
+bool ObjectiveFunction::targetReached()
+{
+  bool targetReached = true;
+  for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++)
+    targetReached &= (*it)->targetReached();
+  return targetReached;
+}
+
+
+std::string ObjectiveFunction::failMeasures()
+{
+  std::string fail;
+  for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++)
+    if ((*it)->fail()) {
+      if (fail.empty()) fail = (*it)->getMeasureName();
+      else fail += " " + (*it)->getMeasureName();
+    }
+  return fail;
+}
+
+
+std::string ObjectiveFunction::targetsNotReached()
+{
+  std::string fail;
+  for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++)
+    if ((*it)->fail()) {
+      if (fail.empty()) fail = (*it)->getName();
+      else fail += " " + (*it)->getName();
+    }
+  return fail;
+}
+
+
+bool ObjectiveFunction::compute(double &obj, alglib::real_1d_array &gradObj)
+{
+  obj = 0.;
+  for (int i = 0; i < gradObj.length(); i++) gradObj[i] = 0.;
+  bool ok = true;
+  for (std::vector<ObjContrib*>::iterator it=begin(); it!=end(); it++)
+    ok &= (*it)->addContrib(obj, gradObj);
+  return ok;
+}
diff --git a/contrib/MeshOptimizer/MeshOptObjectiveFunction.h b/contrib/MeshOptimizer/MeshOptObjectiveFunction.h
new file mode 100644
index 0000000000000000000000000000000000000000..f74f1368842ac6112e9d2b9437170da95f209bee
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptObjectiveFunction.h
@@ -0,0 +1,32 @@
+// TODO: Copyright
+
+#ifndef _MESHOPTOBJECTIVEFUNCTION_H_
+#define _MESHOPTOBJECTIVEFUNCTION_H_
+
+#include <string>
+#include <vector>
+#include "ap.h"
+
+class ObjContrib;
+class MeshOptResults;
+class Patch;
+
+
+class ObjectiveFunction : public  std::vector<ObjContrib*>                         // Contributions to objective function in each pass
+{
+public:
+  void initialize(Patch *mesh);
+  std::string contribNames();
+  std::string minMaxStr();
+  void updateMinMax();
+  void updateParameters();
+  void updateResults(MeshOptResults &res);
+  bool stagnated();
+  bool targetReached();
+  std::string failMeasures();
+  std::string targetsNotReached();
+  bool compute(double &obj, alglib::real_1d_array &gradObj);
+};
+
+
+#endif /* _MESHOPTOBJECTIVEFUNCTION_H_ */
diff --git a/contrib/MeshOptimizer/MeshOptPatch.cpp b/contrib/MeshOptimizer/MeshOptPatch.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8b9f9d585961eb180382899d0a2abc127c9e267d
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptPatch.cpp
@@ -0,0 +1,211 @@
+// Copyright (C) 2013 ULg-UCL
+//
+// Permission is hereby granted, free of charge, to any person
+// obtaining a copy of this software and associated documentation
+// files (the "Software"), to deal in the Software without
+// restriction, including without limitation the rights to use, copy,
+// modify, merge, publish, distribute, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished
+// to do so, provided that the above copyright notice(s) and this
+// permission notice appear in all copies of the Software and that
+// both the above copyright notice(s) and this permission notice
+// appear in supporting documentation.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
+// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
+// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
+// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+// OF THIS SOFTWARE.
+//
+// Please report all bugs and problems to the public mailing list
+// <gmsh@geuz.org>.
+//
+// Contributors: Thomas Toulorge, Jonathan Lambrechts
+
+#include "GmshMessage.h"
+#include "GRegion.h"
+#include "MTriangle.h"
+#include "MQuadrangle.h"
+#include "MTetrahedron.h"
+#include "BasisFactory.h"
+//#include "OptHomIntegralBoundaryDist.h"
+#include "MeshOptPatch.h"
+
+
+Patch::Patch(const std::map<MElement*,GEntity*> &element2entity,
+           const std::set<MElement*> &els, std::set<MVertex*> &toFix,
+           bool fixBndNodes)
+{
+
+  _dim = (*els.begin())->getDim();
+
+  // Initialize elements, vertices, free vertices and element->vertices
+  // connectivity
+  const int nElements = els.size();
+  _nPC = 0;
+  _el.resize(nElements);
+  _el2FV.resize(nElements);
+  _el2V.resize(nElements);
+  _nNodEl.resize(nElements);
+  _indPCEl.resize(nElements);
+  int iEl = 0;
+  bool nonGeoMove = false;
+  for(std::set<MElement*>::const_iterator it = els.begin();
+      it != els.end(); ++it, ++iEl) {
+    _el[iEl] = *it;
+    _nNodEl[iEl] = _el[iEl]->getNumVertices();
+    for (int iVEl = 0; iVEl < _nNodEl[iEl]; iVEl++) {
+      MVertex *vert = _el[iEl]->getVertex(iVEl);
+      GEntity *ge = vert->onWhat();
+      const int vDim = ge->dim();
+      const bool hasParam = ge->haveParametrization();
+      int iV = addVert(vert);
+      _el2V[iEl].push_back(iV);
+      if ((vDim > 0) && (toFix.find(vert) == toFix.end()) && (!fixBndNodes || vDim == _dim)) {   // Free vertex?
+        VertexCoord *coord;
+        if (vDim == 3) coord = new VertexCoordPhys3D();
+        else if (hasParam) coord = new VertexCoordParent(vert);
+        else {
+          if (vDim == 2) coord = new VertexCoordLocalSurf(vert);
+          else coord = new VertexCoordLocalLine(vert);
+          nonGeoMove = true;
+        }
+        int iFV = addFreeVert(vert, iV, vDim, coord, toFix);
+        _el2FV[iEl].push_back(iFV);
+        for (int i=_startPCFV[iFV]; i<_startPCFV[iFV]+vDim; i++) _indPCEl[iEl].push_back(i);
+      }
+      else _el2FV[iEl].push_back(-1);
+    }
+  }
+
+  if (nonGeoMove) Msg::Warning("Some vertices will be moved along local lines "
+                            "or planes, they may not remain on the exact geometry");
+
+  // Initial coordinates
+  _ixyz.resize(nVert());
+  for (int iV = 0; iV < nVert(); iV++) _ixyz[iV] = _vert[iV]->point();
+  _iuvw.resize(nFV());
+  for (int iFV = 0; iFV < nFV(); iFV++) _iuvw[iFV] = _coordFV[iFV]->getUvw(_freeVert[iFV]);
+
+  // Set current coordinates
+  _xyz = _ixyz;
+  _uvw = _iuvw;
+}
+
+
+int Patch::addVert(MVertex* vert)
+{
+  std::vector<MVertex*>::iterator itVert = find(_vert.begin(),_vert.end(),vert);
+  if (itVert == _vert.end()) {
+    _vert.push_back(vert);
+    return _vert.size()-1;
+  }
+  else return std::distance(_vert.begin(),itVert);
+
+}
+
+
+int Patch::addFreeVert(MVertex* vert, const int iV, const int nPCV,
+                      VertexCoord *param, std::set<MVertex*> &toFix)
+{
+  std::vector<MVertex*>::iterator itVert = find(_freeVert.begin(),
+                                                _freeVert.end(),vert);
+  if (itVert == _freeVert.end()) {
+    const int iStart = (_startPCFV.size() == 0)? 0 : _startPCFV.back()+_nPCFV.back();
+    const bool forcedV = (vert->onWhat()->dim() < 2) || (toFix.find(vert) != toFix.end());
+    _freeVert.push_back(vert);
+    _coordFV.push_back(param);
+    _fv2V.push_back(iV);
+    _startPCFV.push_back(iStart);
+    _nPCFV.push_back(nPCV);
+    _nPC += nPCV;
+    _forced.push_back(forcedV);
+    return _freeVert.size()-1;
+  }
+  else return std::distance(_freeVert.begin(),itVert);
+
+}
+
+
+void Patch::getUvw(double *it)
+{
+  for (int iFV = 0; iFV < nFV(); iFV++) {
+    SPoint3 &uvwV = _uvw[iFV];
+    *it = uvwV[0]; it++;
+    if (_nPCFV[iFV] >= 2) { *it = uvwV[1]; it++; }
+    if (_nPCFV[iFV] == 3) { *it = uvwV[2]; it++; }
+  }
+
+}
+
+
+void Patch::updateMesh(const double *it)
+{
+  for (int iFV = 0; iFV < nFV(); iFV++) {
+    int iV = _fv2V[iFV];
+    SPoint3 &uvwV = _uvw[iFV];
+    uvwV[0] = *it; it++;
+    if (_nPCFV[iFV] >= 2) { uvwV[1] = *it; it++; }
+    if (_nPCFV[iFV] == 3) { uvwV[2] = *it; it++; }
+    _xyz[iV] = _coordFV[iFV]->uvw2Xyz(uvwV);
+  }
+
+}
+
+
+void Patch::updateGEntityPositions()
+{
+  for (int iV = 0; iV < nVert(); iV++)
+    _vert[iV]->setXYZ(_xyz[iV].x(),_xyz[iV].y(),_xyz[iV].z());
+  for (int iFV = 0; iFV < nFV(); iFV++)
+    _coordFV[iFV]->exportVertexCoord(_uvw[iFV]);
+}
+
+
+void Patch::pcScale(int iFV, std::vector<double> &scale)
+{
+  // Calc. derivative of x, y & z w.r.t. parametric coordinates
+  const SPoint3 dX(1.,0.,0.), dY(0.,1.,0.), dZ(0.,0.,1.);
+  SPoint3 gX, gY, gZ;
+  _coordFV[iFV]->gXyz2gUvw(_uvw[iFV],dX,gX);
+  _coordFV[iFV]->gXyz2gUvw(_uvw[iFV],dY,gY);
+  _coordFV[iFV]->gXyz2gUvw(_uvw[iFV],dZ,gZ);
+
+  // Scale = inverse norm. of vector (dx/du, dy/du, dz/du)
+  scale[0] = 1./sqrt(gX[0]*gX[0]+gY[0]*gY[0]+gZ[0]*gZ[0]);
+  if (_nPCFV[iFV] >= 2) scale[1] = 1./sqrt(gX[1]*gX[1]+gY[1]*gY[1]+gZ[1]*gZ[1]);
+  if (_nPCFV[iFV] == 3) scale[2] = 1./sqrt(gX[2]*gX[2]+gY[2]*gY[2]+gZ[2]*gZ[2]);
+}
+
+
+void Patch::writeMSH(const char *filename)
+{
+  FILE *f = fopen(filename, "w");
+
+  fprintf(f, "$MeshFormat\n");
+  fprintf(f, "2.2 0 8\n");
+  fprintf(f, "$EndMeshFormat\n");
+
+  fprintf(f, "$Nodes\n");
+  fprintf(f, "%d\n", nVert());
+  for (int i = 0; i < nVert(); i++)
+    fprintf(f, "%d %22.15E %22.15E %22.15E\n", i + 1, _xyz[i].x(), _xyz[i].y(), _xyz[i].z());
+  fprintf(f, "$EndNodes\n");
+
+  fprintf(f, "$Elements\n");
+  fprintf(f, "%d\n", nEl());
+  for (int iEl = 0; iEl < nEl(); iEl++) {
+    fprintf(f, "%d %d 2 0 0", _el[iEl]->getNum(), _el[iEl]->getTypeForMSH());
+    for (size_t iVEl = 0; iVEl < _el2V[iEl].size(); iVEl++)
+      fprintf(f, " %d", _el2V[iEl][iVEl] + 1);
+    fprintf(f, "\n");
+  }
+  fprintf(f, "$EndElements\n");
+
+  fclose(f);
+}
diff --git a/contrib/MeshOptimizer/MeshOptPatch.h b/contrib/MeshOptimizer/MeshOptPatch.h
new file mode 100644
index 0000000000000000000000000000000000000000..2c1d43df71818411d4b32586be49a521f5bc1e52
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptPatch.h
@@ -0,0 +1,132 @@
+// Copyright (C) 2013 ULg-UCL
+//
+// Permission is hereby granted, free of charge, to any person
+// obtaining a copy of this software and associated documentation
+// files (the "Software"), to deal in the Software without
+// restriction, including without limitation the rights to use, copy,
+// modify, merge, publish, distribute, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished
+// to do so, provided that the above copyright notice(s) and this
+// permission notice appear in all copies of the Software and that
+// both the above copyright notice(s) and this permission notice
+// appear in supporting documentation.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
+// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
+// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
+// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+// OF THIS SOFTWARE.
+//
+// Please report all bugs and problems to the public mailing list
+// <gmsh@geuz.org>.
+//
+// Contributors: Thomas Toulorge, Jonathan Lambrechts
+
+#ifndef _MESHOPTPATCH_H_
+#define _MESHOPTPATCH_H_
+
+#include <vector>
+#include <map>
+#include <set>
+#include "fullMatrix.h"
+#include "SPoint3.h"
+#include "MeshOptVertexCoord.h"
+
+
+class GEntity;
+class MVertex;
+class MElement;
+
+
+class Patch
+{
+public:
+  Patch(const std::map<MElement*,GEntity*> &element2entity,
+        const std::set<MElement*> &els, std::set<MVertex*> &toFix, bool fixBndNodes);
+
+  // Mesh entities and variables
+  inline const int &dim() { return _dim; }
+  inline const int &nPC() { return _nPC; }
+  inline int nVert() { return _vert.size(); }
+  inline int nFV() { return _freeVert.size(); }
+  inline const bool forced(int iV) { return _forced[iV]; }
+  inline int nEl() { return _el.size(); }
+  inline const int &nPCFV(int iFV) { return _nPCFV[iFV]; }
+  inline int indPCFV(int iFV, int iPC) { return _startPCFV[iFV]+iPC; }
+  inline int nPCEl(int iEl) { return _indPCEl[iEl].size(); }
+  inline const int &nNodEl(int iEl) { return _nNodEl[iEl]; }
+  inline const int &indPCEl(int iEl, int iPC) { return _indPCEl[iEl][iPC]; }
+  inline const int &el2V(int iEl, int i) { return _el2V[iEl][i]; }
+  inline const int &el2FV(int iEl, int i) { return _el2FV[iEl][i]; }
+  inline MElement *el(int iEl) { return _el[iEl]; }
+  inline const int &fv2V(int iFV) { return _fv2V[iFV]; }
+  inline const SPoint3 &xyz(int iV) { return _xyz[iV]; }
+  inline const SPoint3 &ixyz(int iV) { return _ixyz[iV]; }
+  inline const SPoint3 &uvwFV(int iFV) { return _uvw[iFV]; }
+  inline void gXyz2gUvw(int iFV, const SPoint3 &gXyz, SPoint3 &gUvw);
+  inline void gXyz2gUvw(int iFV, const std::vector<SPoint3> &gXyz,
+                                        std::vector<SPoint3> &gUvw);
+  void pcScale(int iFV, std::vector<double> &scale);                                      // Calc. scale of param. coord. for precond.
+  void getUvw(double *it);
+  void updateMesh(const double *it);
+  void updateGEntityPositions();
+  void writeMSH(const char *filename);
+
+private:
+
+  // Mesh entities and variables
+  int _dim;
+  int _nPC;                                         // Total nb. of parametric coordinates
+  std::vector<MElement*> _el;                       // List of elements
+  std::vector<MVertex*> _vert, _freeVert;           // List of vert., free vert.
+  std::vector<int> _fv2V;                           // Index of free vert. -> index of vert.
+  std::vector<bool> _forced;                        // Is vertex forced?
+  std::vector<SPoint3> _xyz, _ixyz;                 // Physical coord. of ALL vertices (current, straight, init.)
+  std::vector<SPoint3> _uvw, _iuvw;                 // Parametric coord. of FREE vertices (current, straight, init.)
+  std::vector<int> _startPCFV;                      // Start index of parametric coordinates for a free vertex
+  std::vector<int> _nPCFV;                          // Number of parametric coordinates for a free vertex
+  std::vector<std::vector<int> > _el2FV, _el2V;     // Free vertices, vertices in element
+  std::vector<int> _nNodEl;                         // Number of Bezier poly. and nodes for an el.
+  std::vector<std::vector<int> > _indPCEl;          // Index of parametric coord. for an el.
+  std::vector<VertexCoord*> _coordFV;               // Parametrization for a free vertex
+  int addVert(MVertex* vert);
+  int addFreeVert(MVertex* vert, const int iV, const int nPCV,
+                  VertexCoord *param, std::set<MVertex*> &toFix);
+  static inline int indJB2DBase(int nNod, int l, int i, int j)
+  {
+    return (l*nNod+i)*nNod+j;
+  }
+  inline int indJB2D(int iEl, int l, int i, int j)
+  {
+    return indJB2DBase(_nNodEl[iEl],l,i,j);
+  }
+  static inline int indJB3DBase(int nNod, int l, int i, int j, int m)
+  {
+    return ((l*nNod+i)*nNod+j)*nNod+m;
+  }
+  inline int indJB3D(int iEl, int l, int i, int j, int m)
+  {
+    return indJB3DBase(_nNodEl[iEl],l,i,j,m);
+  }
+};
+
+
+inline void Patch::gXyz2gUvw(int iFV, const SPoint3 &gXyz, SPoint3 &gUvw)
+{
+  _coordFV[iFV]->gXyz2gUvw(_uvw[iFV], gXyz, gUvw);
+}
+
+
+inline void Patch::gXyz2gUvw(int iFV, const std::vector<SPoint3> &gXyz,
+                                            std::vector<SPoint3> &gUvw)
+{
+  _coordFV[iFV]->gXyz2gUvw(_uvw[iFV], gXyz, gUvw);
+}
+
+
+#endif
diff --git a/contrib/MeshOptimizer/MeshOptVertexCoord.cpp b/contrib/MeshOptimizer/MeshOptVertexCoord.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..073e97ce216b4a3391b926df1af9f51210729ad7
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptVertexCoord.cpp
@@ -0,0 +1,223 @@
+// Copyright (C) 2013 UCL
+//
+// Permission is hereby granted, free of charge, to any person
+// obtaining a copy of this software and associated documentation
+// files (the "Software"), to deal in the Software without
+// restriction, including without limitation the rights to use, copy,
+// modify, merge, publish, distribute, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished
+// to do so, provided that the above copyright notice(s) and this
+// permission notice appear in all copies of the Software and that
+// both the above copyright notice(s) and this permission notice
+// appear in supporting documentation.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
+// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
+// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
+// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+// OF THIS SOFTWARE.
+//
+// Please report all bugs and problems to the public mailing list
+// <gmsh@geuz.org>.
+//
+// Contributor(s): Thomas Toulorge, Jonathan Lambrechts
+
+#include <iostream>
+#include <algorithm>
+#include "GmshMessage.h"
+#include "GFace.h"
+#include "GEdge.h"
+#include "MVertex.h"
+#include "MLine.h"
+#include "MeshOptVertexCoord.h"
+
+SPoint3 VertexCoordParent::getUvw(MVertex* vert) const
+{
+  GEntity *ge = vert->onWhat();
+  if ((ge->geomType() == GEntity::DiscreteCurve) ||
+      (ge->geomType() == GEntity::DiscreteSurface))
+    Msg::Error("Using parent coordinates on discrete curve or surface");
+
+  switch (ge->dim()) {
+  case 1: {
+    SPoint3 p(0.,0.,0.);
+    reparamMeshVertexOnEdge(vert,static_cast<GEdge*>(ge),p[0]);     // Overkill if vert. well classified and parametrized
+    return p;
+    break;
+  }
+  case 2: {
+    SPoint2 p;
+    reparamMeshVertexOnFace(vert,static_cast<GFace*>(ge),p);      // Overkill if vert. well classified and parametrized
+    return SPoint3(p[0],p[1],0.);
+    break;
+  }
+  }
+  return SPoint3(0.,0.,0.);
+}
+
+SPoint3 VertexCoordParent::uvw2Xyz(const SPoint3 &uvw) const
+{
+  GEntity *ge = _vert->onWhat();
+  GPoint gp = (ge->dim() == 1) ? static_cast<GEdge*>(ge)->point(uvw[0]) :
+                                 static_cast<GFace*>(ge)->point(uvw[0],uvw[1]);
+  return SPoint3(gp.x(),gp.y(),gp.z());
+}
+
+void VertexCoordParent::gXyz2gUvw(const SPoint3 &uvw, const SPoint3 &gXyz, SPoint3 &gUvw) const
+{
+
+  GEntity *ge = _vert->onWhat();
+
+  if (ge->dim() == 1) {
+    SVector3 der = static_cast<GEdge*>(ge)->firstDer(uvw[0]);
+    gUvw[0] = gXyz.x() * der.x() + gXyz.y() * der.y() + gXyz.z() * der.z();
+  }
+  else {
+    Pair<SVector3,SVector3> der = static_cast<GFace*>(ge)->firstDer(SPoint2(uvw[0],uvw[1]));
+    gUvw[0] = gXyz.x() * der.first().x() + gXyz.y() * der.first().y() +
+      gXyz.z() * der.first().z();
+    gUvw[1] = gXyz.x() * der.second().x() + gXyz.y() * der.second().y() +
+      gXyz.z() * der.second().z();
+  }
+
+}
+
+void VertexCoordParent::gXyz2gUvw(const SPoint3 &uvw,
+                                 const std::vector<SPoint3> &gXyz, std::vector<SPoint3> &gUvw) const
+{
+
+  GEntity *ge = _vert->onWhat();
+
+  if (ge->dim() == 1) {
+    SVector3 der = static_cast<GEdge*>(ge)->firstDer(uvw[0]);
+    std::vector<SPoint3>::iterator itUvw = gUvw.begin();
+    for (std::vector<SPoint3>::const_iterator itXyz=gXyz.begin();
+         itXyz != gXyz.end(); itXyz++) {
+      (*itUvw)[0] = itXyz->x() * der.x() + itXyz->y() * der.y() + itXyz->z() * der.z();
+      itUvw++;
+    }
+  }
+  else {
+    Pair<SVector3,SVector3> der = static_cast<GFace*>(ge)->firstDer
+      (SPoint2(uvw[0],uvw[1]));
+    std::vector<SPoint3>::iterator itUvw=gUvw.begin();
+    for (std::vector<SPoint3>::const_iterator itXyz=gXyz.begin();
+         itXyz != gXyz.end(); itXyz++) {
+      (*itUvw)[0] = itXyz->x() * der.first().x() + itXyz->y() * der.first().y() +
+        itXyz->z() * der.first().z();
+      (*itUvw)[1] = itXyz->x() * der.second().x() + itXyz->y() * der.second().y() +
+        itXyz->z() * der.second().z();
+      itUvw++;
+    }
+  }
+
+}
+
+namespace {
+
+SVector3 getLineElTangent(MElement *el, int iNode) {
+
+  double gsf[1256][3], u, v, w;
+  el->getNode(iNode,u,v,w);
+//  el->getGradShapeFunctions(u,v,w,gsf);
+  el->getGradShapeFunctions(u,v,w,gsf,1);
+
+  SVector3 dxyzdu(0.);
+//  int nSF = el->getNumShapeFunctions()();
+  int nSF = el->getNumPrimaryVertices();
+  for (int j=0; j<nSF; j++) {
+    const SPoint3 p = el->getVertex(j)->point();
+    dxyzdu(0) += gsf[j][0]*p.x();
+    dxyzdu(1) += gsf[j][0]*p.y();
+    dxyzdu(2) += gsf[j][0]*p.z();
+  }
+  dxyzdu.normalize();
+
+  return dxyzdu;
+
+}
+
+SVector3 getSurfElNormal(MElement *el, int iNode) {
+
+  double gsf[1256][3], u, v, w;
+  el->getNode(iNode,u,v,w);
+//  el->getGradShapeFunctions(u,v,w,gsf);
+  el->getGradShapeFunctions(u,v,w,gsf,1);
+
+  SVector3 dxyzdu(0.), dxyzdv(0.);
+//  int nSF = el->getNumShapeFunctions()();
+  int nSF = el->getNumPrimaryVertices();
+  for (int j=0; j<nSF; j++) {
+    const SPoint3 p = el->getVertex(j)->point();
+    dxyzdu(0) += gsf[j][0]*p.x();
+    dxyzdu(1) += gsf[j][0]*p.y();
+    dxyzdu(2) += gsf[j][0]*p.z();
+    dxyzdv(0) += gsf[j][1]*p.x();
+    dxyzdv(1) += gsf[j][1]*p.y();
+    dxyzdv(2) += gsf[j][1]*p.z();
+  }
+
+  SVector3 normal = crossprod(dxyzdu,dxyzdv);
+  normal.normalize();
+  return normal;
+
+}
+
+}
+
+VertexCoordLocalLine::VertexCoordLocalLine(MVertex* v) :
+    dir(0.), x0(v->x()), y0(v->y()), z0(v->z())
+{
+
+  GEntity *ge = v->onWhat();
+  const unsigned nEl = ge->getNumMeshElements();
+
+  for (unsigned iEl = 0; iEl < nEl; iEl++) {
+    MElement *el = ge->getMeshElement(iEl);
+    std::vector<MVertex*> lVerts;
+    el->getVertices(lVerts);
+    std::vector<MVertex*>::iterator itV = std::find(lVerts.begin(),lVerts.end(),v);
+    if (itV != lVerts.end()) {
+      const int iNode = std::distance(lVerts.begin(),itV);
+      dir += getLineElTangent(el,iNode);
+    }
+  }
+  dir.normalize();
+
+}
+
+VertexCoordLocalSurf::VertexCoordLocalSurf(MVertex* v) : x0(v->x()), y0(v->y()), z0(v->z())
+{
+
+  GEntity *ge = v->onWhat();
+  const unsigned nEl = ge->getNumMeshElements();
+
+  SVector3 n(0.);
+  for (unsigned iEl = 0; iEl < nEl; iEl++) {
+    MElement *el = ge->getMeshElement(iEl);
+    std::vector<MVertex*> lVerts;
+    el->getVertices(lVerts);
+    std::vector<MVertex*>::iterator itV = std::find(lVerts.begin(),lVerts.end(),v);
+    if (itV != lVerts.end()) {
+      const int iNode = std::distance(lVerts.begin(),itV);
+      n += getSurfElNormal(el,iNode);
+    }
+  }
+  n.normalize();
+
+  if (fabs(fabs(dot(n,SVector3(1.,0.,0.)))-1.) < 1.e-10) {    // If normal is x-axis, take y- and z- axis as dir.
+    dir0 = SVector3(0.,1.,0.);
+    dir1 = SVector3(0.,0.,1.);
+  }
+  else {
+    dir0 = SVector3(1.-n.x()*n.x(),-n.x()*n.y(),-n.x()*n.z());  // 1st dir. = (normalized) proj. of e_x in plane
+    dir0.normalize();
+    dir1 = crossprod(dir0,n);
+  }
+
+}
diff --git a/contrib/MeshOptimizer/MeshOptVertexCoord.h b/contrib/MeshOptimizer/MeshOptVertexCoord.h
new file mode 100644
index 0000000000000000000000000000000000000000..3a1c00df068a1e17323238f2befdc549a8a28fd5
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptVertexCoord.h
@@ -0,0 +1,144 @@
+// Copyright (C) 2013 ULg-UCL
+//
+// Permission is hereby granted, free of charge, to any person
+// obtaining a copy of this software and associated documentation
+// files (the "Software"), to deal in the Software without
+// restriction, including without limitation the rights to use, copy,
+// modify, merge, publish, distribute, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished
+// to do so, provided that the above copyright notice(s) and this
+// permission notice appear in all copies of the Software and that
+// both the above copyright notice(s) and this permission notice
+// appear in supporting documentation.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
+// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
+// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
+// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+// OF THIS SOFTWARE.
+//
+// Please report all bugs and problems to the public mailing list
+// <gmsh@geuz.org>.
+//
+// Contributors: Thomas Toulorge, Jonathan Lambrechts
+
+#ifndef _VERTEXCOORD_H_
+#define _VERTEXCOORD_H_
+
+#include "SPoint3.h"
+#include "GEntity.h"
+#include "MVertex.h"
+
+
+class VertexCoord
+{
+public:
+  // Set param. coord. of MVertex if applicable
+  virtual void exportVertexCoord(const SPoint3 &uvw) {}
+  // Get parametric coordinates of vertex
+  virtual SPoint3 getUvw(MVertex* v) const = 0;
+  // Calculate physical coordinates from parametric coordinates of vertex
+  virtual SPoint3 uvw2Xyz(const SPoint3 &uvw) const = 0;
+  // Calculate derivatives w.r.t parametric coordinates
+  virtual void gXyz2gUvw(const SPoint3 &uvw, const SPoint3 &gXyz, SPoint3 &gUvw) const = 0;
+  // Calculate derivatives w.r.t parametric coordinates
+  virtual void gXyz2gUvw(const SPoint3 &uvw,
+                         const std::vector<SPoint3> &gXyz, std::vector<SPoint3> &gUvw) const = 0;
+  virtual ~VertexCoord() {}
+};
+
+class VertexCoordPhys3D : public VertexCoord
+{
+public:
+  SPoint3 getUvw(MVertex* v) const { return v->point(); }
+  SPoint3 uvw2Xyz(const SPoint3 &uvw) const { return uvw; }
+  void gXyz2gUvw(const SPoint3 &uvw, const SPoint3 &gXyz,
+                         SPoint3 &gUvw) const { gUvw = gXyz; }
+  void gXyz2gUvw(const SPoint3 &uvw, const std::vector<SPoint3> &gXyz, std::vector<SPoint3> &gUvw) const
+  {
+    std::vector<SPoint3>::iterator itUvw=gUvw.begin();
+    for (std::vector<SPoint3>::const_iterator itXyz=gXyz.begin(); itXyz != gXyz.end();
+         itXyz++) {
+      *itUvw = *itXyz;
+      itUvw++;
+    }
+  }
+};
+
+
+class VertexCoordParent : public VertexCoord
+{
+public:
+  VertexCoordParent(MVertex* v) : _vert(v) {}
+  void exportVertexCoord(const SPoint3 &uvw) {
+    for (int d = 0; d < _vert->onWhat()->dim(); ++d) _vert->setParameter(d, uvw[d]);
+  }
+  SPoint3 getUvw(MVertex* v) const;
+  SPoint3 uvw2Xyz(const SPoint3 &uvw) const;
+  void gXyz2gUvw(const SPoint3 &uvw, const SPoint3 &gXyz, SPoint3 &gUvw) const;
+  void gXyz2gUvw(const SPoint3 &uvw, const std::vector<SPoint3> &gXyz, std::vector<SPoint3> &gUvw) const;
+protected:
+  MVertex *_vert;
+};
+
+
+class VertexCoordLocalLine : public VertexCoord
+{
+public:
+  VertexCoordLocalLine(MVertex* v);
+  SPoint3 getUvw(MVertex* v) const { return SPoint3(0.,0.,0.); }
+  SPoint3 uvw2Xyz(const SPoint3 &uvw) const {
+    return SPoint3(x0+uvw[0]*dir[0],y0+uvw[0]*dir[1],z0+uvw[0]*dir[2]);
+  }
+  void gXyz2gUvw(const SPoint3 &uvw, const SPoint3 &gXyz, SPoint3 &gUvw) const {
+    gUvw[0] = gXyz.x()*dir[0] + gXyz.y()*dir[1] + gXyz.z()*dir[2];
+  }
+  void gXyz2gUvw(const SPoint3 &uvw, const std::vector<SPoint3> &gXyz, std::vector<SPoint3> &gUvw) const {
+    std::vector<SPoint3>::iterator itUvw = gUvw.begin();
+    for (std::vector<SPoint3>::const_iterator itXyz=gXyz.begin();
+         itXyz != gXyz.end(); itXyz++) {
+      (*itUvw)[0] = itXyz->x()*dir[0] + itXyz->y()*dir[1] + itXyz->z()*dir[2];
+      itUvw++;
+    }
+  }
+protected:
+  double x0, y0, z0;
+  SVector3 dir;
+};
+
+
+class VertexCoordLocalSurf : public VertexCoord
+{
+public:
+  VertexCoordLocalSurf(MVertex* v);
+  SPoint3 getUvw(MVertex* v) const { return SPoint3(0.,0.,0.); }
+  SPoint3 uvw2Xyz(const SPoint3 &uvw) const {
+    return SPoint3(x0+uvw[0]*dir0[0]+uvw[1]*dir1[0],
+                   y0+uvw[0]*dir0[1]+uvw[1]*dir1[1],
+                   z0+uvw[0]*dir0[2]+uvw[1]*dir1[2]);
+  }
+  void gXyz2gUvw(const SPoint3 &uvw, const SPoint3 &gXyz, SPoint3 &gUvw) const {
+    gUvw[0] = gXyz.x()*dir0[0] + gXyz.y()*dir0[1] + gXyz.z()*dir0[2];
+    gUvw[1] = gXyz.x()*dir1[0] + gXyz.y()*dir1[1] + gXyz.z()*dir1[2];
+  }
+  void gXyz2gUvw(const SPoint3 &uvw, const std::vector<SPoint3> &gXyz, std::vector<SPoint3> &gUvw) const {
+    std::vector<SPoint3>::iterator itUvw = gUvw.begin();
+    for (std::vector<SPoint3>::const_iterator itXyz=gXyz.begin();
+         itXyz != gXyz.end(); itXyz++) {
+      (*itUvw)[0] = itXyz->x()*dir0[0] + itXyz->y()*dir0[1] + itXyz->z()*dir0[2];
+      (*itUvw)[1] = itXyz->x()*dir1[0] + itXyz->y()*dir1[1] + itXyz->z()*dir1[2];
+      itUvw++;
+    }
+  }
+protected:
+  double x0, y0, z0;
+  SVector3 dir0, dir1;
+};
+
+
+#endif
diff --git a/contrib/MeshOptimizer/MeshOptimizer.cpp b/contrib/MeshOptimizer/MeshOptimizer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52f7a37cf21a1b06e9218ea0391cd24783f8c94a
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptimizer.cpp
@@ -0,0 +1,473 @@
+// Copyright (C) 2013 ULg-UCL
+//
+// Permission is hereby granted, free of charge, to any person
+// obtaining a copy of this software and associated documentation
+// files (the "Software"), to deal in the Software without
+// restriction, including without limitation the rights to use, copy,
+// modify, merge, publish, distribute, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished
+// to do so, provided that the above copyright notice(s) and this
+// permission notice appear in all copies of the Software and that
+// both the above copyright notice(s) and this permission notice
+// appear in supporting documentation.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
+// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
+// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
+// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+// OF THIS SOFTWARE.
+//
+// Please report all bugs and problems to the public mailing list
+// <gmsh@geuz.org>.
+//
+// Contributors: Thomas Toulorge, Jonathan Lambrechts
+
+#include <stdio.h>
+#include <sstream>
+#include <iterator>
+#include <string.h>
+#include <stack>
+#include "GmshConfig.h"
+#include "Gmsh.h"
+#include "GModel.h"
+#include "MTriangle.h"
+#include "MQuadrangle.h"
+#include "MTetrahedron.h"
+#include "MHexahedron.h"
+#include "MPrism.h"
+#include "MLine.h"
+#include "OS.h"
+#include "MeshOpt.h"
+#include "MeshOptCommon.h"
+#include "MeshOptimizer.h"
+
+#if defined(HAVE_BFGS)
+
+
+static std::set<MVertex *> getAllBndVertices
+  (std::set<MElement*> &elements,
+   const std::map<MVertex*, std::vector<MElement*> > &vertex2elements)
+{
+  std::set<MVertex*> bnd;
+  for (std::set<MElement*>::iterator itE = elements.begin(); itE != elements.end(); ++itE) {
+    for (int i = 0; i < (*itE)->getNumPrimaryVertices(); ++i) {
+      const std::vector<MElement*> &neighbours = vertex2elements.find
+        ((*itE)->getVertex(i))->second;
+      for (size_t k = 0; k < neighbours.size(); ++k) {
+        if (elements.find(neighbours[k]) == elements.end()) {
+          for (int j = 0; j < neighbours[k]->getNumVertices(); ++j) {
+            bnd.insert(neighbours[k]->getVertex(j));
+          }
+        }
+      }
+    }
+  }
+  return bnd;
+}
+
+
+// Approximate test of intersection element with circle/sphere by sampling
+static bool testElInDist(const SPoint3 p, double limDist, MElement *el)
+{
+  const double sampleLen = 0.5*limDist;                                   // Distance between sample points
+
+  if (el->getDim() == 2) {                                                // 2D?
+    for (int iEd = 0; iEd < el->getNumEdges(); iEd++) {                   // Loop over edges of element
+      MEdge ed = el->getEdge(iEd);
+      const int nPts = int(ed.length()/sampleLen)+2;                      // Nb of sample points based on edge length
+      for (int iPt = 0; iPt < nPts; iPt++) {                              // Loop over sample points
+        const SPoint3 pt = ed.interpolate(iPt/float(nPts-1));
+        if (p.distance(pt) < limDist) return true;
+      }
+    }
+  }
+  else {                                                                  // 3D
+    for (int iFace = 0; iFace < el->getNumFaces(); iFace++) {             // Loop over faces of element
+      MFace face = el->getFace(iFace);
+      double lMax = 0.;                                                   // Max. edge length in face
+      const int nVert = face.getNumVertices();
+      for (int iEd = 0; iEd < nVert; iEd++)
+        lMax = std::max(lMax, face.getEdge(iEd).length());
+      const int nPts = int(lMax/sampleLen)+2;                             // Nb of sample points based on max. edge length in face
+      for (int iPt0 = 0; iPt0 < nPts; iPt0++) {
+        const double u = iPt0/float(nPts-1);
+        for (int iPt1 = 0; iPt1 < nPts; iPt1++) {                         // Loop over sample points
+          const double vMax = (nVert == 3) ? 1.-u : 1.;
+          const SPoint3 pt = face.interpolate(u, vMax*iPt1/float(nPts-1));
+          if (p.distance(pt) < limDist) return true;
+        }
+      }
+    }
+  }
+
+  return false;
+}
+
+
+static std::set<MElement*> getSurroundingPatch(MElement *el, int minLayers,
+                            int maxLayers, double limDist,
+                            const std::map<MVertex*, std::vector<MElement*> > &vertex2elements)
+{
+
+  const SPoint3 pnt = el->barycenter(true);
+
+  std::set<MElement*> patch;
+  std::list<MElement*> currentLayer, lastLayer;
+
+  patch.insert(el);
+  lastLayer.push_back(el);
+  for (int d = 0; d < maxLayers; ++d) {
+    currentLayer.clear();
+    for (std::list<MElement*>::iterator it = lastLayer.begin();
+         it != lastLayer.end(); ++it) {
+      for (int i = 0; i < (*it)->getNumPrimaryVertices(); ++i) {
+        const std::vector<MElement*> &neighbours = vertex2elements.find
+          ((*it)->getVertex(i))->second;
+        for (std::vector<MElement*>::const_iterator itN = neighbours.begin();
+             itN != neighbours.end(); ++itN){
+          if ((d < minLayers) || testElInDist(pnt, limDist, *itN))
+            if (patch.insert(*itN).second) currentLayer.push_back(*itN);  // Assume that if an el is too far, its neighbours are too far as well
+        }
+      }
+    }
+    lastLayer = currentLayer;
+  }
+
+  return patch;
+}
+
+
+static void addPatchChaintoGroup(std::set<int> &group,
+                                const std::vector<std::set<int> > &groupConnect,
+                                std::vector<bool> &todoPB, int iB)
+{
+  if (todoPB[iB]) {
+    todoPB[iB] = false;
+    group.insert(iB);
+    const std::set<int> &connect = groupConnect[iB];
+    for (std::set<int>::const_iterator itBC = connect.begin(); itBC != connect.end(); ++itBC)
+      addPatchChaintoGroup(group, groupConnect, todoPB, *itBC);
+  }
+}
+
+
+static void calcVertex2Elements(int dim, GEntity *entity,
+                                std::map<MVertex*, std::vector<MElement *> > &vertex2elements)
+{
+  for (size_t i = 0; i < entity->getNumMeshElements(); ++i) {
+    MElement *element = entity->getMeshElement(i);
+    if (element->getDim() == dim)
+      for (int j = 0; j < element->getNumPrimaryVertices(); ++j)
+        vertex2elements[element->getVertex(j)].push_back(element);
+  }
+}
+
+
+static void calcElement2Entity(GEntity *entity, std::map<MElement*,GEntity*> &element2entity)
+{
+  for (size_t i = 0; i < entity->getNumMeshElements(); ++i) {
+    MElement *element = entity->getMeshElement(i);
+    element2entity.insert(std::pair<MElement*,GEntity*>(element,entity));
+  }
+}
+
+
+static std::vector<std::pair<std::set<MElement*>, std::set<MVertex*> > > getConnectedPatches
+  (const std::map<MVertex*, std::vector<MElement *> > &vertex2elements,
+   const std::set<MElement*> &badElements, const MeshOptParameters &par)
+{
+
+  Msg::Info("Starting patch generation from %i bad elements...", badElements.size());
+
+  // Contruct primary patches
+  Msg::Info("Constructing %i primary patches", badElements.size());
+  std::vector<std::set<MElement*> > primPatches;
+  primPatches.reserve(badElements.size());
+  for (std::set<MElement*>::const_iterator it = badElements.begin(); it != badElements.end(); ++it) {
+    const double limDist = par.patch->maxDistance(*it);
+    primPatches.push_back(getSurroundingPatch(*it, par.patch->minLayers,
+                            par.patch->maxLayers, limDist, vertex2elements));
+  }
+
+  // Compute patch connectivity
+  Msg::Info("Computing patch connectivity...");
+  std::map<MElement*, std::set<int> > tags;
+  std::vector<std::set<int> > patchConnect(primPatches.size());
+  for (int iB = 0; iB < primPatches.size(); ++iB) {
+    std::set<MElement*> &patch = primPatches[iB];
+    for(std::set<MElement*>::iterator itEl = patch.begin(); itEl != patch.end(); ++itEl) {
+      std::set<int> &patchInd = tags[*itEl];
+      if (!patchInd.empty() && (badElements.find(*itEl) != badElements.end() ||
+                               !par.patch->weakMerge)) {
+        for (std::set<int>::iterator itBS = patchInd.begin();
+             itBS != patchInd.end(); ++itBS) patchConnect[*itBS].insert(iB);
+        patchConnect[iB].insert(patchInd.begin(), patchInd.end());
+      }
+      patchInd.insert(iB);
+    }
+  }
+
+  // Identify groups of connected patches
+  Msg::Info("Identifying groups of primary patches...");
+  std::list<std::set<int> > groups;
+  std::vector<bool> todoPB(primPatches.size(), true);
+  for (int iB = 0; iB < primPatches.size(); ++iB)
+    if (todoPB[iB]) {
+      std::set<int> group;
+      addPatchChaintoGroup(group, patchConnect, todoPB, iB);
+      groups.push_back(group);
+    }
+
+  // Merge primary patches according to groups
+  Msg::Info("Merging primary patches into %i patches...", groups.size());
+  std::list<std::set<MElement*> > patches;
+  for (std::list<std::set<int> >::iterator itG = groups.begin();
+       itG != groups.end(); ++itG) {
+    patches.push_back(std::set<MElement*>());
+    for (std::set<int>::iterator itB = itG->begin(); itB != itG->end(); ++itB) {
+      std::set<MElement*> primPatch = primPatches[*itB];
+      patches.back().insert(primPatch.begin(), primPatch.end());
+    }
+  }
+
+  // Store and compute patch boundaries
+  Msg::Info("Computing boundaries for %i patches...", patches.size());
+  std::vector<std::pair<std::set<MElement *>, std::set<MVertex*> > > result;
+  for (std::list<std::set<MElement*> >::iterator itB = patches.begin();
+       itB != patches.end(); ++itB)
+    result.push_back(std::pair<std::set<MElement*>, std::set<MVertex*> >
+                     (*itB, getAllBndVertices(*itB, vertex2elements)));
+
+  Msg::Info("Generated %i patches", patches.size());
+
+  return result;
+}
+
+
+static void optimizeConnectedPatches
+  (const std::map<MVertex*, std::vector<MElement *> > &vertex2elements,
+   const std::map<MElement*,GEntity*> &element2entity,
+   std::set<MElement*> &badasses,
+   MeshOptParameters &par, MeshOptResults &res)
+{
+  res.success = 1;
+
+  std::vector<std::pair<std::set<MElement*>, std::set<MVertex*> > > toOptimize =
+                              getConnectedPatches(vertex2elements, badasses, par);
+
+  for (int iPatch = 0; iPatch < toOptimize.size(); ++iPatch) {
+
+    // Initialize optimization and output if asked
+    if (par.verbose > 1)
+      Msg::Info("Optimizing patch %i/%i composed of %i elements", iPatch,
+                      toOptimize.size()-1, toOptimize[iPatch].first.size());
+    MeshOpt opt(element2entity, toOptimize[iPatch].first, toOptimize[iPatch].second,
+                                                                      par.fixBndNodes);
+    if (par.verbose > 3) {
+      std::ostringstream ossI1;
+      ossI1 << "initial_patch-" << iPatch << ".msh";
+      opt.patch.writeMSH(ossI1.str().c_str());
+    }
+
+    // Optimize patch
+    int success = -1;
+    if (opt.patch.nPC() > 0)
+      success = opt.optimize(par);
+    else
+      if (par.verbose > 1) Msg::Info("Patch %i has no degree of freedom, skipping", iPatch);
+
+    if (par.verbose > 3) {
+      std::ostringstream ossI2;
+      ossI2 << "final_patch-" << iPatch << ".msh";
+      opt.patch.writeMSH(ossI2.str().c_str());
+    }
+
+    // Evaluate mesh and update it if (partial) success
+    opt.updateResults(res);
+    if (success >= 0) opt.patch.updateGEntityPositions();
+
+    //#pragma omp critical
+    res.success = std::min(res.success, success);
+  }
+
+}
+
+
+static MElement *getWorstElement(std::set<MElement*> &badElts, const MeshOptParameters &par)
+{
+  double worst = 1.e300;
+  MElement *worstEl = 0;
+
+  for (std::set<MElement*>::iterator it=badElts.begin(); it!=badElts.end(); it++) {
+    const double val = par.patch->elBadness(*it);
+    if (val < worst) {
+      worst = val;
+      worstEl = *it;
+    }
+  }
+
+  return worstEl;
+}
+
+
+static void optimizeOneByOne
+  (const std::map<MVertex*, std::vector<MElement *> > &vertex2elements,
+   const std::map<MElement*,GEntity*> &element2entity,
+   std::set<MElement*> badElts, MeshOptParameters &par, MeshOptResults &res)
+{
+  res.success = 1;
+
+  const int initNumBadElts = badElts.size();
+  if (par.verbose > 0) Msg::Info("%d bad elements, starting to iterate...", initNumBadElts);
+
+  // Loop over bad elements
+  for (int iBadEl=0; iBadEl<initNumBadElts; iBadEl++) {
+
+    if (badElts.empty()) break;
+
+    // Create patch around worst element and remove it from badElts
+    MElement *worstEl = getWorstElement(badElts, par);
+    badElts.erase(worstEl);
+
+    // Initialize patch size to be adapted
+    int maxLayers = par.patch->maxLayers;
+    double distanceFactor = 1.;
+    int success;
+
+    // Patch adaptation loop
+    for (int iAdapt=0; iAdapt<par.patch->maxAdaptPatch; iAdapt++) {
+
+      // Set up patch
+      const double limDist = par.patch->maxDistance(worstEl);
+      std::set<MElement*> toOptimizePrim = getSurroundingPatch(worstEl, par.patch->minLayers,
+                                                          maxLayers, limDist, vertex2elements);
+      std::set<MVertex*> toFix = getAllBndVertices(toOptimizePrim, vertex2elements);
+      std::set<MElement*> toOptimize;
+      std::set_difference(toOptimizePrim.begin(),toOptimizePrim.end(),
+                          badElts.begin(),badElts.end(),
+                          std::inserter(toOptimize, toOptimize.end()));
+
+      // Initialize optimization and output if asked
+      if (par.verbose > 1)
+        Msg::Info("Optimizing patch %i (max. %i remaining) composed of %4d elements",
+                                            iBadEl, badElts.size(), toOptimize.size());
+      MeshOpt opt(element2entity, toOptimize, toFix, par.fixBndNodes);
+      if (par.verbose > 3) {
+        std::ostringstream ossI1;
+        ossI1 << "initial_patch-" << iBadEl << ".msh";
+        opt.patch.writeMSH(ossI1.str().c_str());
+      }
+
+      // Optimize patch
+      if (opt.patch.nPC() == 0) {
+        success = -1;
+        Msg::Info("Patch %i (adapt #%i) has no degree of freedom, skipping", iBadEl, iAdapt);
+      }
+      else
+        success = opt.optimize(par);
+
+      // Output if asked
+      if (par.verbose > 3) {
+        std::ostringstream ossI2;
+        ossI2 << "final_patch-" << iBadEl << "_adapt-" << iAdapt <<".msh";
+        opt.patch.writeMSH(ossI2.str().c_str());
+      }
+
+      // If (partial) success, update mesh and break adaptation loop, otherwise adapt
+      if ((success > 0) || (iAdapt == par.patch->maxAdaptPatch-1)) {
+        opt.updateResults(res);
+        if (success >= 0) {
+          opt.patch.updateGEntityPositions();
+          break;
+        }
+        else {
+          distanceFactor *= par.patch->distanceAdaptFact;
+          maxLayers *= par.patch->maxLayersAdaptFact;
+          if (par.verbose > 1)
+            Msg::Info("Patch %i failed (adapt #%i), adapting with increased size", iBadEl, iAdapt);
+        }
+      }
+
+    }                                                                       // End of adaptation loop
+
+    if (par.verbose > 1)
+      switch (success) {
+        case 1: Msg::Info("Patch %i succeeded", iBadEl); break;
+        case 0:
+          Msg::Info("Patch %i partially failed (measure "
+                    "above critical value but below target)", iBadEl);
+          break;
+        case -1: Msg::Info("Patch %i failed", iBadEl); break;
+      }
+
+    res.success = std::min(res.success, success);
+
+  }
+}
+
+#endif
+
+
+void meshOptimizer(GModel *gm, MeshOptParameters &par, MeshOptResults &res)
+{
+#if defined(HAVE_BFGS)
+
+  double startTime = Cpu();
+  if (par.verbose > 0) Msg::StatusBar(true, "Optimizing mesh...");
+
+  std::vector<GEntity*> entities;
+  gm->getEntities(entities);
+
+  std::map<MVertex*, std::vector<MElement *> > vertex2elements;
+  std::map<MElement*,GEntity*> element2entity;
+  std::set<MElement*> badElts;
+  for (int iEnt = 0; iEnt < entities.size(); ++iEnt) {
+    GEntity* &entity = entities[iEnt];
+    if (entity->dim() != par.dim ||
+        (par.onlyVisible && !entity->getVisibility())) continue;
+    Msg::Info("Computing connectivity and bad elements for entity %d...",
+              entity->tag());
+    calcVertex2Elements(par.dim,entity,vertex2elements);
+    for (int iEl = 0; iEl < entity->getNumMeshElements();iEl++) {       // Detect bad elements
+      double jmin, jmax;
+      MElement *el = entity->getMeshElement(iEl);
+//      if (el->getDim() == par.dim) {
+//        if (par.patchJac.test) {
+//          el->scaledJacRange(jmin, jmax);
+//          if (jmin < par.patchJac.min || jmax > par.patchJac.max) badElts.insert(el);
+//        }
+//      }
+      if ((el->getDim() == par.dim) && (par.patch->elBadness(el) < 0.)) badElts.insert(el);
+    }
+  }
+
+  if (par.patch->strategy == MeshOptParameters::STRAT_CONNECTED)
+    optimizeConnectedPatches(vertex2elements, element2entity, badElts, par, res);
+  else if (par.patch->strategy == MeshOptParameters::STRAT_ONEBYONE)
+    optimizeOneByOne(vertex2elements, element2entity, badElts, par, res);
+  else
+    Msg::Error("Unknown strategy %d for mesh optimization", par.patch->strategy);
+
+  if (par.verbose > 0) {
+    if (res.success == 1)
+      Msg::Info("Optimization succeeded");
+    else if (res.success == 0)
+      Msg::Warning("Optimization partially failed (all measures above critical "
+                    "value, but some below target)");
+    else if (res.success == -1)
+      Msg::Error("Optimization failed (some measures below critical value)");
+    res.CPU = Cpu()-startTime;
+    Msg::StatusBar(true, "Done optimizing mesh (%g s)", res.CPU);
+  }
+
+
+#else
+  Msg::Error("Mesh optimizer requires BFGS");
+#endif
+
+}
diff --git a/contrib/MeshOptimizer/MeshOptimizer.h b/contrib/MeshOptimizer/MeshOptimizer.h
new file mode 100644
index 0000000000000000000000000000000000000000..1daf3fe70ab75e789ddc39bd5a5331c8709a29f1
--- /dev/null
+++ b/contrib/MeshOptimizer/MeshOptimizer.h
@@ -0,0 +1,42 @@
+// Copyright (C) 2014 ULg-UCL
+//
+// Permission is hereby granted, free of charge, to any person
+// obtaining a copy of this software and associated documentation
+// files (the "Software"), to deal in the Software without
+// restriction, including without limitation the rights to use, copy,
+// modify, merge, publish, distribute, and/or sell copies of the
+// Software, and to permit persons to whom the Software is furnished
+// to do so, provided that the above copyright notice(s) and this
+// permission notice appear in all copies of the Software and that
+// both the above copyright notice(s) and this permission notice
+// appear in supporting documentation.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT OF THIRD PARTY RIGHTS. IN NO EVENT SHALL THE
+// COPYRIGHT HOLDER OR HOLDERS INCLUDED IN THIS NOTICE BE LIABLE FOR
+// ANY CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY
+// DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
+// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
+// ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
+// OF THIS SOFTWARE.
+//
+// Please report all bugs and problems to the public mailing list
+// <gmsh@geuz.org>.
+//
+// Contributors: Thomas Toulorge, Jonathan Lambrechts
+
+#ifndef _MESHOPTIMIZER_H_
+#define _MESHOPTIMIZER_H_
+
+
+class GModel;
+class MeshOptParameters;
+class MeshOptResults;
+
+
+void meshOptimizer(GModel *gm, MeshOptParameters &par, MeshOptResults &res);
+
+
+#endif