Index: src/builder.cpp
===================================================================
--- src/builder.cpp	(revision 18913c2ca930123026243027483a5e8569fd7118)
+++ src/builder.cpp	(revision 6e93532a2d845fe2e2784e97e1a0882bd15affda)
@@ -993,5 +993,5 @@
               SaveFlag = true;
               cout << Verbose(1) << "Parsing forces file and Verlet integrating." << endl;
-              if (!mol->VerletForceIntegration(argv[argptr], configuration.Deltat, configuration.GetIsAngstroem()))
+              if (!mol->VerletForceIntegration((ofstream *)&cout, argv[argptr], configuration.Deltat, configuration.GetIsAngstroem(), configuration.DoConstrainedMD))
                 cout << Verbose(2) << "File not found." << endl;
               else
Index: src/config.cpp
===================================================================
--- src/config.cpp	(revision 18913c2ca930123026243027483a5e8569fd7118)
+++ src/config.cpp	(revision 6e93532a2d845fe2e2784e97e1a0882bd15affda)
@@ -35,4 +35,5 @@
   DoFullCurrent=0;
   DoWannier=0;
+  DoConstrainedMD=0;
   CommonWannier=0;
   SawtoothStart=0.01;
@@ -517,4 +518,7 @@
   }
   
+  if (ParseForParameter(verbose,file,"DoConstrainedMD", 0, 1, 1, int_type, &(config::DoConstrainedMD), 1, optional))
+    if (config::DoConstrainedMD < 0) 
+      config::DoConstrainedMD = 0;
   ParseForParameter(verbose,file,"MaxOuterStep", 0, 1, 1, int_type, &(config::MaxOuterStep), 1, critical);
   if (!ParseForParameter(verbose,file,"Deltat", 0, 1, 1, double_type, &(config::Deltat), 1, optional))
Index: src/molecules.cpp
===================================================================
--- src/molecules.cpp	(revision 18913c2ca930123026243027483a5e8569fd7118)
+++ src/molecules.cpp	(revision 6e93532a2d845fe2e2784e97e1a0882bd15affda)
@@ -1007,14 +1007,147 @@
 };
 
+/** Evaluates the potential energy used for constrained molecular dynamics.
+ * \f$V_i^{con} = c^{bond} \cdot | r_{P(i)} - R_i | + sum_{i \neq j} C^{min} \cdot \tfrac{1}{C_{ij}} + C^{inj} \Bigl (1 - \theta \bigl (\prod_{i \neq j} (P(i) - P(j)) \bigr ) \Bigr )\f$
+ *     where the first term points to the target in minimum distance, the second is a penalty for trajectories lying too close to each other (\f$C_{ij}$ is minimum distance between 
+ *     trajectories i and j) and the third term is a penalty for two atoms trying to each the same target point.
+ * Note that for the second term we have to solve the following linear system:
+ * \f$-c_1 \cdot n_1 + c_2 \cdot n_2 + C \cdot n_3 = - p_2 + p_1\f$, where \f$c_1\f$, \f$c_2\f$ and \f$C\f$ are constants, 
+ * offset vector \f$p_1\f$ in direction \f$n_1\f$, offset vector \f$p_2\f$ in direction \f$n_2\f$,
+ * \f$n_3\f$ is the normal vector to both directions. \f$C\f$ would be the minimum distance between the two lines.
+ * \sa molecule::MinimiseConstrainedPotential(), molecule::VerletForceIntegration()
+ * \param *out output stream for debugging
+ * \param *permutation gives target index for each atom, array of size molecule::AtomCount (this is "x" in \f$V^{con}(x)\f$)
+ * \param startstep start configuration (MDStep in molecule::trajectories)
+ * \param endstep end configuration (MDStep in molecule::trajectories)
+ * \param *constants constant in front of each term
+ * \param IsAngstroem whether coordinates are in angstroem (true) or bohrradius (false)
+ * \return potential energy
+ * \note This routine is scaling quadratically which is not optimal.
+ */
+double molecule::ConstrainedPotential(ofstream *out, int *permutation, int startstep, int endstep, double *constants, bool IsAngstroem)
+{
+  double result = 0., tmp;
+  atom *Walker = NULL, *Runner = NULL, *Sprinter = NULL;
+  Vector trajectory1, trajectory2, normal, TestVector;
+  gsl_matrix *A = gsl_matrix_alloc(NDIM,NDIM);
+  gsl_vector *x = gsl_vector_alloc(NDIM);
+
+  // go through every atom
+  Walker = start;
+  while (Walker->next != end) {
+    Walker = Walker->next;
+    // first term: distance to target
+    Runner = FindAtom(permutation[Walker->nr]);   // find target point
+    result += constants[0] * (Trajectories[Walker].R.at(startstep).Distance(&Trajectories[Runner].R.at(endstep)));
+    
+    // second term: sum of distances to other trajectories
+    Runner = start;
+    while (Runner->next != end) {
+      Runner = Runner->next;
+      // determine normalized trajectories direction vector (n1, n2)
+      Sprinter = FindAtom(permutation[Walker->nr]);   // find target point
+      trajectory1.CopyVector(&Trajectories[Sprinter].R.at(endstep));
+      trajectory1.SubtractVector(&Trajectories[Walker].R.at(startstep));
+      trajectory1.Normalize();
+      Sprinter = FindAtom(permutation[Runner->nr]);   // find target point
+      trajectory2.CopyVector(&Trajectories[Sprinter].R.at(endstep));
+      trajectory2.SubtractVector(&Trajectories[Runner].R.at(startstep));
+      trajectory2.Normalize();
+      // check whether they're linear dependent
+      if (fabs(trajectory1.ScalarProduct(&trajectory2)/trajectory1.Norm()/trajectory2.Norm() - 1.) < MYEPSILON) {
+        *out << "Both trajectories of " << *Walker << " and " << *Runner << " are linear dependent: ";
+        *out << trajectory1;
+        *out << " and ";
+        *out << trajectory2 << endl;
+        tmp = Trajectories[Walker].R.at(startstep).Distance(&Trajectories[Runner].R.at(startstep));
+      } else { // determine distance by finding minimum distance
+        *out << "Both trajectories of " << *Walker << " and " << *Runner << " are linear independent." << endl;
+        // determine normal vector for both
+        normal.MakeNormalVector(&trajectory1, &trajectory2);
+        // setup matrix
+        for (int i=NDIM;i--;) {
+          gsl_matrix_set(A, 0, i, trajectory1.x[i]);
+          gsl_matrix_set(A, 1, i, trajectory2.x[i]);
+          gsl_matrix_set(A, 2, i, normal.x[i]);
+          gsl_vector_set(x,i, (Trajectories[Walker].R.at(startstep).x[i] - Trajectories[Runner].R.at(startstep).x[i]));
+        }
+        // solve the linear system by Householder transformations
+        gsl_linalg_HH_svx(A, x);
+        // distance from last component
+        tmp = gsl_vector_get(x,2);   
+        // test whether we really have the intersection (by checking on c_1 and c_2)
+        TestVector.CopyVector(&Trajectories[Runner].R.at(startstep));
+        trajectory2.Scale(gsl_vector_get(x,1));
+        TestVector.AddVector(&trajectory2);
+        normal.Scale(gsl_vector_get(x,2));
+        TestVector.AddVector(&normal);
+        TestVector.SubtractVector(&Trajectories[Walker].R.at(startstep));
+        trajectory1.Scale(gsl_vector_get(x,0));
+        TestVector.SubtractVector(&trajectory1);
+        if (TestVector.Norm() < MYEPSILON) {
+          *out << "Test: ok.\tDistance of " << tmp << " is correct." << endl;  
+        } else {
+          *out << "Test: failed.\tDifference in intersection is ";
+          *out << TestVector;
+          *out << "." << endl;  
+        }
+      }
+      // add up
+      result += constants[1] * 1./tmp;
+    }
+      
+    // third term: penalty for equal targets
+    Runner = start;
+    while (Runner->next != end) {
+      Runner = Runner->next;
+      if ((permutation[Walker->nr] == permutation[Runner->nr]) && (Walker->nr < Runner->nr)) {
+        Sprinter = FindAtom(permutation[Walker->nr]);
+        *out << *Walker << " and " << *Runner << " are heading to the same target at ";
+        *out << Trajectories[Sprinter].R.at(endstep);
+        *out << ", penalting." << endl;
+        result += constants[2];
+      }
+    }
+  }
+  
+  return result;
+};
+
+/** Minimises the extra potential for constrained molecular dynamics and gives forces and the constrained potential energy.
+ * We do the following:
+ *  -# First, we minimise the potential, see molecule::ConstrainedPotential()
+ *  -# Then, we calculate the forces by taking the spatial derivative, where we scale the potential to such a degree,
+ *     that the total force is always pointing in direction of the constraint force (ensuring that we move in the
+ *     right direction).
+ *  -# Finally, we calculate the potential energy and return.
+ * \param *out output stream for debugging
+ * \param *Force ForceMatrix containing force vectors from the external energy functional minimisation.
+ * \param targetstep target step between which and the predecessor we perform the constrained MD
+ * \sa molecule::VerletForceIntegration()
+ * \return potential energy
+ */
+double molecule::MinimiseConstrainedPotential(ofstream *out, ForceMatrix *Force, int targetstep)
+{
+  double potential = 0.;
+  // Minimise the potential
+  
+  // evaluate forces
+  
+  // evaluate potential and return
+  return potential;
+};
+
 /** Parses nuclear forces from file and performs Verlet integration.
  * Note that we assume the parsed forces to be in atomic units (hence, if coordinates are in angstroem, we
  * have to transform them).
  * This adds a new MD step to the config file.
+ * \param *out output stream for debugging
  * \param *file filename
  * \param delta_t time step width in atomic units
  * \param IsAngstroem whether coordinates are in angstroem (true) or bohrradius (false)
+ * \param DoConstrained whether we perform a constrained (>0, target step in molecule::trajectories) or unconstrained (0) molecular dynamics, \sa molecule::MinimiseConstrainedPotential() 
  * \return true - file found and parsed, false - file not found or imparsable
  */
-bool molecule::VerletForceIntegration(char *file, double delta_t, bool IsAngstroem)
+bool molecule::VerletForceIntegration(ofstream *out, char *file, double delta_t, bool IsAngstroem, int DoConstrained)
 {
   element *runner = elemente->start;
@@ -1024,5 +1157,5 @@
   string token;
   stringstream item;
-  double a, IonMass, Vector[NDIM];
+  double a, IonMass, Vector[NDIM], ConstrainedPotentialEnergy;
   ForceMatrix Force;
 
@@ -1053,4 +1186,9 @@
         Force.Matrix[0][i][d+5] -= Vector[d]/(double)AtomCount;
       }
+    // solve a constrained potential if we are meant to
+    if (DoConstrained) {
+      ConstrainedPotentialEnergy = MinimiseConstrainedPotential(out, &Force, DoConstrained);
+    }
+    
     // and perform Verlet integration for each atom with position, velocity and force vector
     runner = elemente->start;
@@ -1067,5 +1205,5 @@
             // check size of vectors
             if (Trajectories[walker].R.size() <= (unsigned int)(MDSteps)) {
-              //cout << "Increasing size for trajectory array of " << *walker << " to " << (size+10) << "." << endl;
+              //out << "Increasing size for trajectory array of " << *walker << " to " << (size+10) << "." << endl;
               Trajectories[walker].R.resize(MDSteps+10);
               Trajectories[walker].U.resize(MDSteps+10);
@@ -1085,11 +1223,11 @@
               Trajectories[walker].U.at(MDSteps).x[d] += a * (Trajectories[walker].F.at(MDSteps).x[d]+Trajectories[walker].F.at(MDSteps-1).x[d]);
             }
-//            cout << "Integrated position&velocity of step " << (MDSteps) << ": ("; 
+//            out << "Integrated position&velocity of step " << (MDSteps) << ": ("; 
 //            for (int d=0;d<NDIM;d++)
-//              cout << Trajectories[walker].R.at(MDSteps).x[d] << " ";          // next step
-//            cout << ")\t(";
+//              out << Trajectories[walker].R.at(MDSteps).x[d] << " ";          // next step
+//            out << ")\t(";
 //            for (int d=0;d<NDIM;d++)
 //              cout << Trajectories[walker].U.at(MDSteps).x[d] << " ";          // next step
-//            cout << ")" << endl;
+//            out << ")" << endl;
             // next atom
             AtomNo++;
Index: src/molecules.hpp
===================================================================
--- src/molecules.hpp	(revision 18913c2ca930123026243027483a5e8569fd7118)
+++ src/molecules.hpp	(revision 6e93532a2d845fe2e2784e97e1a0882bd15affda)
@@ -10,9 +10,10 @@
 
 // GSL headers
+#include <gsl/gsl_eigen.h>
+#include <gsl/gsl_heapsort.h>
+#include <gsl/gsl_linalg.h>
+#include <gsl/gsl_matrix.h>
 #include <gsl/gsl_multimin.h>
 #include <gsl/gsl_vector.h>
-#include <gsl/gsl_matrix.h>
-#include <gsl/gsl_eigen.h>
-#include <gsl/gsl_heapsort.h>
 
 // STL headers
@@ -259,5 +260,7 @@
 	void PrincipalAxisSystem(ofstream *out, bool DoRotate);
 	double VolumeOfConvexEnvelope(ofstream *out, bool IsAngstroem);
-	bool VerletForceIntegration(char *file, double delta_t, bool IsAngstroem);
+	bool VerletForceIntegration(ofstream *out, char *file, double delta_t, bool IsAngstroem, int DoConstrained);
+	double ConstrainedPotential(ofstream *out, int *permutation, int start, int end, double *constants, bool IsAngstroem);
+	double MinimiseConstrainedPotential(ofstream *out, ForceMatrix *Force, int targetstep);
 	
   bool CheckBounds(const Vector *x) const;
@@ -385,4 +388,5 @@
     bool FastParsing;
     double Deltat;
+    int DoConstrainedMD;
     
     private:
