Index: src/atom.cpp
===================================================================
--- src/atom.cpp	(revision c111dbf810e0e3f7f92d4afa3b50cd427f3d515d)
+++ src/atom.cpp	(revision ccd9f5b1e4f784fe73721ff1b3b2c5f78ec8df8b)
@@ -9,4 +9,5 @@
 #include "element.hpp"
 #include "memoryallocator.hpp"
+#include "parser.hpp"
 #include "vector.hpp"
 
@@ -297,2 +298,16 @@
 };
 
+/** Evaluates some constraint potential if atom moves from \a startstep at once to \endstep in trajectory.
+ * \param startstep trajectory begins at
+ * \param endstep trajectory ends at
+ * \param **PermutationMap if atom switches places with some other atom, there is no translation but a permutaton noted here (not in the trajectories of each).
+ * \param *Force Force matrix to store result in
+ */
+void atom::EvaluateConstrainedForce(int startstep, int endstep, atom **PermutationMap, ForceMatrix *Force)
+{
+  double constant = 10.;
+  atom *Sprinter = PermutationMap[nr];
+  // set forces
+  for (int i=NDIM;i++;)
+    Force->Matrix[0][nr][5+i] += 2.*constant*sqrt(Trajectory.R.at(startstep).Distance(&Sprinter->Trajectory.R.at(endstep)));
+};
Index: src/atom.hpp
===================================================================
--- src/atom.hpp	(revision c111dbf810e0e3f7f92d4afa3b50cd427f3d515d)
+++ src/atom.hpp	(revision ccd9f5b1e4f784fe73721ff1b3b2c5f78ec8df8b)
@@ -27,4 +27,5 @@
 class bond;
 class element;
+class ForceMatrix;
 class Vector;
 
@@ -83,4 +84,5 @@
 
   void AddKineticToTemperature(double *temperature, int step) const;
+  void EvaluateConstrainedForce(int startstep, int endstep, atom **PermutationMap, ForceMatrix *Force);
 
   bool IsInParallelepiped(Vector offset, double *parallelepiped);
Index: src/molecule.hpp
===================================================================
--- src/molecule.hpp	(revision c111dbf810e0e3f7f92d4afa3b50cd427f3d515d)
+++ src/molecule.hpp	(revision ccd9f5b1e4f784fe73721ff1b3b2c5f78ec8df8b)
@@ -56,5 +56,18 @@
 /************************************* Class definitions ****************************************/
 
-
+/** Structure to contain parameters needed for evaluation of constraint potential.
+ */
+struct EvaluatePotential
+{
+  int startstep;              //!< start configuration (MDStep in atom::trajectory)
+  int endstep;                //!< end configuration (MDStep in atom::trajectory)
+  atom **PermutationMap;      //!< gives target ptr for each atom, array of size molecule::AtomCount (this is "x" in \f$ V^{con}(x) \f$ )
+  DistanceMap **DistanceList; //!< distance list of each atom to each atom
+  DistanceMap::iterator *StepList; //!< iterator to ascend through NearestNeighbours \a **DistanceList
+  int *DoubleList;            //!< count of which sources want to move to this target, basically the injective measure (>1 -> not injective)
+  DistanceMap::iterator *DistanceIterators; //!< marks which was the last picked target as injective candidate with smallest distance
+  bool IsAngstroem;           //!< whether coordinates are in angstroem (true) or bohrradius (false)
+  double *PenaltyConstants;   //!<  penalty constant in front of each term
+};
 
 #define MaxThermostats 6      //!< maximum number of thermostat entries in Ions#ThermostatNames and Ions#ThermostatImplemented 
@@ -216,5 +229,5 @@
 
 
-  double ConstrainedPotential(ofstream *out, atom **permutation, int start, int end, double *constants, bool IsAngstroem);
+  double ConstrainedPotential(ofstream *out, struct EvaluatePotential &Params);
   double MinimiseConstrainedPotential(ofstream *out, atom **&permutation, int startstep, int endstep, bool IsAngstroem);
   void EvaluateConstrainedForces(ofstream *out, int startstep, int endstep, atom **PermutationMap, ForceMatrix *Force);
Index: src/molecule_dynamics.cpp
===================================================================
--- src/molecule_dynamics.cpp	(revision c111dbf810e0e3f7f92d4afa3b50cd427f3d515d)
+++ src/molecule_dynamics.cpp	(revision ccd9f5b1e4f784fe73721ff1b3b2c5f78ec8df8b)
@@ -15,4 +15,132 @@
 /************************************* Functions for class molecule *********************************/
 
+/** Penalizes long trajectories.
+ * \param *Walker atom to check against others
+ * \param *mol molecule with other atoms
+ * \param &Params constraint potential parameters
+ * \return penalty times each distance
+ */
+double SumDistanceOfTrajectories(atom *Walker, molecule *mol, struct EvaluatePotential &Params)
+{
+  gsl_matrix *A = gsl_matrix_alloc(NDIM,NDIM);
+  gsl_vector *x = gsl_vector_alloc(NDIM);
+  atom * Runner = mol->start;
+  atom *Sprinter = NULL;
+  Vector trajectory1, trajectory2, normal, TestVector;
+  double Norm1, Norm2, tmp, result = 0.;
+
+  while (Runner->next != mol->end) {
+    Runner = Runner->next;
+    if (Runner == Walker) // hence, we only go up to the Walker, not beyond (similar to i=0; i<j; i++)
+      break;
+    // determine normalized trajectories direction vector (n1, n2)
+    Sprinter = Params.PermutationMap[Walker->nr];   // find first target point
+    trajectory1.CopyVector(&Sprinter->Trajectory.R.at(Params.endstep));
+    trajectory1.SubtractVector(&Walker->Trajectory.R.at(Params.startstep));
+    trajectory1.Normalize();
+    Norm1 = trajectory1.Norm();
+    Sprinter = Params.PermutationMap[Runner->nr];   // find second target point
+    trajectory2.CopyVector(&Sprinter->Trajectory.R.at(Params.endstep));
+    trajectory2.SubtractVector(&Runner->Trajectory.R.at(Params.startstep));
+    trajectory2.Normalize();
+    Norm2 = trajectory1.Norm();
+    // check whether either is zero()
+    if ((Norm1 < MYEPSILON) && (Norm2 < MYEPSILON)) {
+      tmp = Walker->Trajectory.R.at(Params.startstep).Distance(&Runner->Trajectory.R.at(Params.startstep));
+    } else if (Norm1 < MYEPSILON) {
+      Sprinter = Params.PermutationMap[Walker->nr];   // find first target point
+      trajectory1.CopyVector(&Sprinter->Trajectory.R.at(Params.endstep));  // copy first offset
+      trajectory1.SubtractVector(&Runner->Trajectory.R.at(Params.startstep));  // subtract second offset
+      trajectory2.Scale( trajectory1.ScalarProduct(&trajectory2) ); // trajectory2 is scaled to unity, hence we don't need to divide by anything
+      trajectory1.SubtractVector(&trajectory2);   // project the part in norm direction away
+      tmp = trajectory1.Norm();  // remaining norm is distance
+    } else if (Norm2 < MYEPSILON) {
+      Sprinter = Params.PermutationMap[Runner->nr];   // find second target point
+      trajectory2.CopyVector(&Sprinter->Trajectory.R.at(Params.endstep));  // copy second offset
+      trajectory2.SubtractVector(&Walker->Trajectory.R.at(Params.startstep));  // subtract first offset
+      trajectory1.Scale( trajectory2.ScalarProduct(&trajectory1) ); // trajectory1 is scaled to unity, hence we don't need to divide by anything
+      trajectory2.SubtractVector(&trajectory1);   // project the part in norm direction away
+      tmp = trajectory2.Norm();  // remaining norm is distance
+    } else if ((fabs(trajectory1.ScalarProduct(&trajectory2)/Norm1/Norm2) - 1.) < MYEPSILON) { // check whether they're linear dependent
+  //        *out << Verbose(3) << "Both trajectories of " << *Walker << " and " << *Runner << " are linear dependent: ";
+  //        *out << trajectory1;
+  //        *out << " and ";
+  //        *out << trajectory2;
+      tmp = Walker->Trajectory.R.at(Params.startstep).Distance(&Runner->Trajectory.R.at(Params.startstep));
+  //        *out << " with distance " << tmp << "." << endl;
+    } else { // determine distance by finding minimum distance
+  //        *out << Verbose(3) << "Both trajectories of " << *Walker << " and " << *Runner << " are linear independent ";
+  //        *out << endl;
+  //        *out << "First Trajectory: ";
+  //        *out << trajectory1 << endl;
+  //        *out << "Second Trajectory: ";
+  //        *out << trajectory2 << endl;
+      // determine normal vector for both
+      normal.MakeNormalVector(&trajectory1, &trajectory2);
+      // print all vectors for debugging
+  //        *out << "Normal vector in between: ";
+  //        *out << normal << endl;
+      // 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, (Walker->Trajectory.R.at(Params.startstep).x[i] - Runner->Trajectory.R.at(Params.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);
+  //        *out << " with distance " << tmp << "." << endl;
+      // test whether we really have the intersection (by checking on c_1 and c_2)
+      TestVector.CopyVector(&Runner->Trajectory.R.at(Params.startstep));
+      trajectory2.Scale(gsl_vector_get(x,1));
+      TestVector.AddVector(&trajectory2);
+      normal.Scale(gsl_vector_get(x,2));
+      TestVector.AddVector(&normal);
+      TestVector.SubtractVector(&Walker->Trajectory.R.at(Params.startstep));
+      trajectory1.Scale(gsl_vector_get(x,0));
+      TestVector.SubtractVector(&trajectory1);
+      if (TestVector.Norm() < MYEPSILON) {
+  //          *out << Verbose(2) << "Test: ok.\tDistance of " << tmp << " is correct." << endl;
+      } else {
+  //          *out << Verbose(2) << "Test: failed.\tIntersection is off by ";
+  //          *out << TestVector;
+  //          *out << "." << endl;
+      }
+    }
+    // add up
+    tmp *= Params.IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
+    if (fabs(tmp) > MYEPSILON) {
+      result += Params.PenaltyConstants[1] * 1./tmp;
+      //*out << Verbose(4) << "Adding " << 1./tmp*constants[1] << "." << endl;
+    }
+  }
+  return result;
+};
+
+/** Penalizes atoms heading to same target.
+ * \param *Walker atom to check against others
+ * \param *mol molecule with other atoms
+ * \param &Params constrained potential parameters
+ * \return \a penalty times the number of equal targets
+ */
+double PenalizeEqualTargets(atom *Walker, molecule *mol, struct EvaluatePotential &Params)
+{
+  double result = 0.;
+  atom * Runner = mol->start;
+  while (Runner->next != mol->end) {
+    Runner = Runner->next;
+    if ((Params.PermutationMap[Walker->nr] == Params.PermutationMap[Runner->nr]) && (Walker->nr < Runner->nr)) {
+  //    atom *Sprinter = PermutationMap[Walker->nr];
+  //        *out << *Walker << " and " << *Runner << " are heading to the same target at ";
+  //        *out << Sprinter->Trajectory.R.at(endstep);
+  //        *out << ", penalting." << endl;
+      result += Params.PenaltyConstants[2];
+      //*out << Verbose(4) << "Adding " << constants[2] << "." << endl;
+    }
+  }
+  return result;
+};
 
 /** Evaluates the potential energy used for constrained molecular dynamics.
@@ -26,135 +154,30 @@
  * \sa molecule::MinimiseConstrainedPotential(), molecule::VerletForceIntegration()
  * \param *out output stream for debugging
- * \param *PermutationMap gives target ptr 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)
+ * \param &Params constrained potential parameters
  * \return potential energy
  * \note This routine is scaling quadratically which is not optimal.
  * \todo There's a bit double counting going on for the first time, bu nothing to worry really about.
  */
-double molecule::ConstrainedPotential(ofstream *out, atom **PermutationMap, int startstep, int endstep, double *constants, bool IsAngstroem)
-{
-  double result = 0., tmp, Norm1, Norm2;
-  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);
+double molecule::ConstrainedPotential(ofstream *out, struct EvaluatePotential &Params)
+{
+  double tmp, result;
 
   // go through every atom
-  Walker = start;
+  atom *Runner = NULL;
+  atom *Walker = start;
   while (Walker->next != end) {
     Walker = Walker->next;
     // first term: distance to target
-    Runner = PermutationMap[Walker->nr];   // find target point
-    tmp = (Walker->Trajectory.R.at(startstep).Distance(&Runner->Trajectory.R.at(endstep)));
-    tmp *= IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
-    result += constants[0] * tmp;
+    Runner = Params.PermutationMap[Walker->nr];   // find target point
+    tmp = (Walker->Trajectory.R.at(Params.startstep).Distance(&Runner->Trajectory.R.at(Params.endstep)));
+    tmp *= Params.IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
+    result += Params.PenaltyConstants[0] * tmp;
     //*out << Verbose(4) << "Adding " << tmp*constants[0] << "." << endl;
 
     // second term: sum of distances to other trajectories
-    Runner = start;
-    while (Runner->next != end) {
-      Runner = Runner->next;
-      if (Runner == Walker) // hence, we only go up to the Walker, not beyond (similar to i=0; i<j; i++)
-        break;
-      // determine normalized trajectories direction vector (n1, n2)
-      Sprinter = PermutationMap[Walker->nr];   // find first target point
-      trajectory1.CopyVector(&Sprinter->Trajectory.R.at(endstep));
-      trajectory1.SubtractVector(&Walker->Trajectory.R.at(startstep));
-      trajectory1.Normalize();
-      Norm1 = trajectory1.Norm();
-      Sprinter = PermutationMap[Runner->nr];   // find second target point
-      trajectory2.CopyVector(&Sprinter->Trajectory.R.at(endstep));
-      trajectory2.SubtractVector(&Runner->Trajectory.R.at(startstep));
-      trajectory2.Normalize();
-      Norm2 = trajectory1.Norm();
-      // check whether either is zero()
-      if ((Norm1 < MYEPSILON) && (Norm2 < MYEPSILON)) {
-        tmp = Walker->Trajectory.R.at(startstep).Distance(&Runner->Trajectory.R.at(startstep));
-      } else if (Norm1 < MYEPSILON) {
-        Sprinter = PermutationMap[Walker->nr];   // find first target point
-        trajectory1.CopyVector(&Sprinter->Trajectory.R.at(endstep));  // copy first offset
-        trajectory1.SubtractVector(&Runner->Trajectory.R.at(startstep));  // subtract second offset
-        trajectory2.Scale( trajectory1.ScalarProduct(&trajectory2) ); // trajectory2 is scaled to unity, hence we don't need to divide by anything
-        trajectory1.SubtractVector(&trajectory2);   // project the part in norm direction away
-        tmp = trajectory1.Norm();  // remaining norm is distance
-      } else if (Norm2 < MYEPSILON) {
-        Sprinter = PermutationMap[Runner->nr];   // find second target point
-        trajectory2.CopyVector(&Sprinter->Trajectory.R.at(endstep));  // copy second offset
-        trajectory2.SubtractVector(&Walker->Trajectory.R.at(startstep));  // subtract first offset
-        trajectory1.Scale( trajectory2.ScalarProduct(&trajectory1) ); // trajectory1 is scaled to unity, hence we don't need to divide by anything
-        trajectory2.SubtractVector(&trajectory1);   // project the part in norm direction away
-        tmp = trajectory2.Norm();  // remaining norm is distance
-      } else if ((fabs(trajectory1.ScalarProduct(&trajectory2)/Norm1/Norm2) - 1.) < MYEPSILON) { // check whether they're linear dependent
-//        *out << Verbose(3) << "Both trajectories of " << *Walker << " and " << *Runner << " are linear dependent: ";
-//        *out << trajectory1;
-//        *out << " and ";
-//        *out << trajectory2;
-        tmp = Walker->Trajectory.R.at(startstep).Distance(&Runner->Trajectory.R.at(startstep));
-//        *out << " with distance " << tmp << "." << endl;
-      } else { // determine distance by finding minimum distance
-//        *out << Verbose(3) << "Both trajectories of " << *Walker << " and " << *Runner << " are linear independent ";
-//        *out << endl;
-//        *out << "First Trajectory: ";
-//        *out << trajectory1 << endl;
-//        *out << "Second Trajectory: ";
-//        *out << trajectory2 << endl;
-        // determine normal vector for both
-        normal.MakeNormalVector(&trajectory1, &trajectory2);
-        // print all vectors for debugging
-//        *out << "Normal vector in between: ";
-//        *out << normal << endl;
-        // 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, (Walker->Trajectory.R.at(startstep).x[i] - Runner->Trajectory.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);
-//        *out << " with distance " << tmp << "." << endl;
-        // test whether we really have the intersection (by checking on c_1 and c_2)
-        TestVector.CopyVector(&Runner->Trajectory.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(&Walker->Trajectory.R.at(startstep));
-        trajectory1.Scale(gsl_vector_get(x,0));
-        TestVector.SubtractVector(&trajectory1);
-        if (TestVector.Norm() < MYEPSILON) {
-//          *out << Verbose(2) << "Test: ok.\tDistance of " << tmp << " is correct." << endl;
-        } else {
-//          *out << Verbose(2) << "Test: failed.\tIntersection is off by ";
-//          *out << TestVector;
-//          *out << "." << endl;
-        }
-      }
-      // add up
-      tmp *= IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
-      if (fabs(tmp) > MYEPSILON) {
-        result += constants[1] * 1./tmp;
-        //*out << Verbose(4) << "Adding " << 1./tmp*constants[1] << "." << endl;
-      }
-    }
+    result += SumDistanceOfTrajectories(Walker, this, Params);
 
     // third term: penalty for equal targets
-    Runner = start;
-    while (Runner->next != end) {
-      Runner = Runner->next;
-      if ((PermutationMap[Walker->nr] == PermutationMap[Runner->nr]) && (Walker->nr < Runner->nr)) {
-        Sprinter = PermutationMap[Walker->nr];
-//        *out << *Walker << " and " << *Runner << " are heading to the same target at ";
-//        *out << Sprinter->Trajectory.R.at(endstep);
-//        *out << ", penalting." << endl;
-        result += constants[2];
-        //*out << Verbose(4) << "Adding " << constants[2] << "." << endl;
-      }
-    }
+    result += PenalizeEqualTargets(Walker, this, Params);
   }
 
@@ -162,24 +185,133 @@
 };
 
-void PrintPermutationMap(ofstream *out, atom **PermutationMap, int Nr)
+/** print the current permutation map.
+ * \param *out output stream for debugging
+ * \param &Params constrained potential parameters
+ * \param AtomCount number of atoms
+ */
+void PrintPermutationMap(ofstream *out, int AtomCount, struct EvaluatePotential &Params)
 {
   stringstream zeile1, zeile2;
-  int *DoubleList = Malloc<int>(Nr, "PrintPermutationMap: *DoubleList");
+  int *DoubleList = Malloc<int>(AtomCount, "PrintPermutationMap: *DoubleList");
   int doubles = 0;
-  for (int i=0;i<Nr;i++)
+  for (int i=0;i<AtomCount;i++)
     DoubleList[i] = 0;
   zeile1 << "PermutationMap: ";
   zeile2 << "                ";
-  for (int i=0;i<Nr;i++) {
-    DoubleList[PermutationMap[i]->nr]++;
+  for (int i=0;i<AtomCount;i++) {
+    Params.DoubleList[Params.PermutationMap[i]->nr]++;
     zeile1 << i << " ";
-    zeile2 << PermutationMap[i]->nr << " ";
-  }
-  for (int i=0;i<Nr;i++)
-    if (DoubleList[i] > 1)
+    zeile2 << Params.PermutationMap[i]->nr << " ";
+  }
+  for (int i=0;i<AtomCount;i++)
+    if (Params.DoubleList[i] > 1)
     doubles++;
- // *out << "Found " << doubles << " Doubles." << endl;
+  if (doubles >0)
+    *out << "Found " << doubles << " Doubles." << endl;
   Free(&DoubleList);
 //  *out << zeile1.str() << endl << zeile2.str() << endl;
+};
+
+/** \f$O(N^2)\f$ operation of calculation distance between each atom pair and putting into DistanceList.
+ * \param *mol molecule to scan distances in
+ * \param &Params constrained potential parameters
+ */
+void FillDistanceList(molecule *mol, struct EvaluatePotential &Params)
+{
+  for (int i=mol->AtomCount; i--;) {
+    Params.DistanceList[i] = new DistanceMap;    // is the distance sorted target list per atom
+    Params.DistanceList[i]->clear();
+  }
+
+  atom *Runner = NULL;
+  atom *Walker = mol->start;
+  while (Walker->next != mol->end) {
+    Walker = Walker->next;
+    Runner = mol->start;
+    while(Runner->next != mol->end) {
+      Runner = Runner->next;
+      Params.DistanceList[Walker->nr]->insert( DistancePair(Walker->Trajectory.R.at(Params.startstep).Distance(&Runner->Trajectory.R.at(Params.endstep)), Runner) );
+    }
+  }
+};
+
+/** initialize lists.
+ * \param *out output stream for debugging
+ * \param *mol molecule to scan distances in
+ * \param &Params constrained potential parameters
+ */
+void CreateInitialLists(ofstream *out, molecule *mol, struct EvaluatePotential &Params)
+{
+  for (int i=mol->AtomCount; i--;)
+    Params.DoubleList[i] = 0;  // stores for how many atoms in startstep this atom is a target in endstep
+
+  atom *Walker = mol->start;
+  while (Walker->next != mol->end) {
+    Walker = Walker->next;
+    Params.StepList[Walker->nr] = Params.DistanceList[Walker->nr]->begin();    // stores the step to the next iterator that could be a possible next target
+    Params.PermutationMap[Walker->nr] = Params.DistanceList[Walker->nr]->begin()->second;   // always pick target with the smallest distance
+    Params.DoubleList[Params.DistanceList[Walker->nr]->begin()->second->nr]++;            // increase this target's source count (>1? not injective)
+    Params.DistanceIterators[Walker->nr] = Params.DistanceList[Walker->nr]->begin();    // and remember which one we picked
+    *out << *Walker << " starts with distance " << Params.DistanceList[Walker->nr]->begin()->first << "." << endl;
+  }
+};
+
+/** Try the next nearest neighbour in order to make the permutation map injective.
+ * \param *out output stream for debugging
+ * \param *mol molecule
+ * \param *Walker atom to change its target
+ * \param &OldPotential old value of constraint potential to see if we do better with new target
+ * \param &Params constrained potential parameters
+ */
+double TryNextNearestNeighbourForInjectivePermutation(ofstream *out, molecule *mol, atom *Walker, double &OldPotential, struct EvaluatePotential &Params)
+{
+  double Potential = 0;
+  DistanceMap::iterator NewBase = Params.DistanceIterators[Walker->nr];  // store old base
+  do {
+    NewBase++;  // take next further distance in distance to targets list that's a target of no one
+  } while ((Params.DoubleList[NewBase->second->nr] != 0) && (NewBase != Params.DistanceList[Walker->nr]->end()));
+  if (NewBase != Params.DistanceList[Walker->nr]->end()) {
+    Params.PermutationMap[Walker->nr] = NewBase->second;
+    Potential = fabs(mol->ConstrainedPotential(out, Params));
+    if (Potential > OldPotential) { // undo
+      Params.PermutationMap[Walker->nr] = Params.DistanceIterators[Walker->nr]->second;
+    } else {  // do
+      Params.DoubleList[Params.DistanceIterators[Walker->nr]->second->nr]--;  // decrease the old entry in the doubles list
+      Params.DoubleList[NewBase->second->nr]++;    // increase the old entry in the doubles list
+      Params.DistanceIterators[Walker->nr] = NewBase;
+      OldPotential = Potential;
+      *out << Verbose(3) << "Found a new permutation, new potential is " << OldPotential << "." << endl;
+    }
+  }
+  return Potential;
+};
+
+/** Permutes \a **&PermutationMap until the penalty is below constants[2].
+ * \param *out output stream for debugging
+ * \param *mol molecule to scan distances in
+ * \param &Params constrained potential parameters
+ */
+void MakeInjectivePermutation(ofstream *out, molecule *mol, struct EvaluatePotential &Params)
+{
+  atom *Walker = mol->start;
+  DistanceMap::iterator NewBase;
+  double Potential = fabs(mol->ConstrainedPotential(out, Params));
+
+  while ((Potential) > Params.PenaltyConstants[2]) {
+    PrintPermutationMap(out, mol->AtomCount, Params);
+    Walker = Walker->next;
+    if (Walker == mol->end) // round-robin at the end
+      Walker = mol->start->next;
+    if (Params.DoubleList[Params.DistanceIterators[Walker->nr]->second->nr] <= 1)  // no need to make those injective that aren't
+      continue;
+    // now, try finding a new one
+    Potential = TryNextNearestNeighbourForInjectivePermutation(out, mol, Walker, Potential, Params);
+  }
+  for (int i=mol->AtomCount; i--;) // now each single entry in the DoubleList should be <=1
+    if (Params.DoubleList[i] > 1) {
+      cerr << "Failed to create an injective PermutationMap!" << endl;
+      exit(1);
+    }
+  *out << Verbose(1) << "done." << endl;
 };
 
@@ -214,10 +346,10 @@
 {
   double Potential, OldPotential, OlderPotential;
-  PermutationMap = Malloc<atom*>(AtomCount, "molecule::MinimiseConstrainedPotential: **PermutationMap");
-  DistanceMap **DistanceList = Malloc<DistanceMap*>(AtomCount, "molecule::MinimiseConstrainedPotential: **DistanceList");
-  DistanceMap::iterator *DistanceIterators = Malloc<DistanceMap::iterator>(AtomCount, "molecule::MinimiseConstrainedPotential: *DistanceIterators");
-  int *DoubleList = Malloc<int>(AtomCount, "molecule::MinimiseConstrainedPotential: *DoubleList");
-  DistanceMap::iterator *StepList = Malloc<DistanceMap::iterator>(AtomCount, "molecule::MinimiseConstrainedPotential: *StepList");
-  double constants[3];
+  struct EvaluatePotential Params;
+  Params.PermutationMap = Malloc<atom*>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.**PermutationMap");
+  Params.DistanceList = Malloc<DistanceMap*>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.**DistanceList");
+  Params.DistanceIterators = Malloc<DistanceMap::iterator>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.*DistanceIterators");
+  Params.DoubleList = Malloc<int>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.*DoubleList");
+  Params.StepList = Malloc<DistanceMap::iterator>(AtomCount, "molecule::MinimiseConstrainedPotential: Params.*StepList");
   int round;
   atom *Walker = NULL, *Runner = NULL, *Sprinter = NULL;
@@ -226,73 +358,19 @@
   /// Minimise the potential
   // set Lagrange multiplier constants
-  constants[0] = 10.;
-  constants[1] = 1.;
-  constants[2] = 1e+7;    // just a huge penalty
+  Params.PenaltyConstants[0] = 10.;
+  Params.PenaltyConstants[1] = 1.;
+  Params.PenaltyConstants[2] = 1e+7;    // just a huge penalty
   // generate the distance list
-  *out << Verbose(1) << "Creating the distance list ... " << endl;
-  for (int i=AtomCount; i--;) {
-    DoubleList[i] = 0;  // stores for how many atoms in startstep this atom is a target in endstep
-    DistanceList[i] = new DistanceMap;    // is the distance sorted target list per atom
-    DistanceList[i]->clear();
-  }
-  *out << Verbose(1) << "Filling the distance list ... " << endl;
-  Walker = start;
-  while (Walker->next != end) {
-    Walker = Walker->next;
-    Runner = start;
-    while(Runner->next != end) {
-      Runner = Runner->next;
-      DistanceList[Walker->nr]->insert( DistancePair(Walker->Trajectory.R.at(startstep).Distance(&Runner->Trajectory.R.at(endstep)), Runner) );
-    }
-  }
+  *out << Verbose(1) << "Allocating, initializting and filling the distance list ... " << endl;
+  FillDistanceList(this, Params);
+
   // create the initial PermutationMap (source -> target)
-  Walker = start;
-  while (Walker->next != end) {
-    Walker = Walker->next;
-    StepList[Walker->nr] = DistanceList[Walker->nr]->begin();    // stores the step to the next iterator that could be a possible next target
-    PermutationMap[Walker->nr] = DistanceList[Walker->nr]->begin()->second;   // always pick target with the smallest distance
-    DoubleList[DistanceList[Walker->nr]->begin()->second->nr]++;            // increase this target's source count (>1? not injective)
-    DistanceIterators[Walker->nr] = DistanceList[Walker->nr]->begin();    // and remember which one we picked
-    *out << *Walker << " starts with distance " << DistanceList[Walker->nr]->begin()->first << "." << endl;
-  }
-  *out << Verbose(1) << "done." << endl;
+  CreateInitialLists(out, this, Params);
+
   // make the PermutationMap injective by checking whether we have a non-zero constants[2] term in it
   *out << Verbose(1) << "Making the PermutationMap injective ... " << endl;
-  Walker = start;
-  DistanceMap::iterator NewBase;
-  OldPotential = fabs(ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem));
-  while ((OldPotential) > constants[2]) {
-    PrintPermutationMap(out, PermutationMap, AtomCount);
-    Walker = Walker->next;
-    if (Walker == end) // round-robin at the end
-      Walker = start->next;
-    if (DoubleList[DistanceIterators[Walker->nr]->second->nr] <= 1)  // no need to make those injective that aren't
-      continue;
-    // now, try finding a new one
-    NewBase = DistanceIterators[Walker->nr];  // store old base
-    do {
-      NewBase++;  // take next further distance in distance to targets list that's a target of no one
-    } while ((DoubleList[NewBase->second->nr] != 0) && (NewBase != DistanceList[Walker->nr]->end()));
-    if (NewBase != DistanceList[Walker->nr]->end()) {
-      PermutationMap[Walker->nr] = NewBase->second;
-      Potential = fabs(ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem));
-      if (Potential > OldPotential) { // undo
-        PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second;
-      } else {  // do
-        DoubleList[DistanceIterators[Walker->nr]->second->nr]--;  // decrease the old entry in the doubles list
-        DoubleList[NewBase->second->nr]++;    // increase the old entry in the doubles list
-        DistanceIterators[Walker->nr] = NewBase;
-        OldPotential = Potential;
-        *out << Verbose(3) << "Found a new permutation, new potential is " << OldPotential << "." << endl;
-      }
-    }
-  }
-  for (int i=AtomCount; i--;) // now each single entry in the DoubleList should be <=1
-    if (DoubleList[i] > 1) {
-      cerr << "Failed to create an injective PermutationMap!" << endl;
-      exit(1);
-    }
-  *out << Verbose(1) << "done." << endl;
-  Free(&DoubleList);
+  MakeInjectivePermutation(out, this, Params);
+  Free(&Params.DoubleList);
+
   // argument minimise the constrained potential in this injective PermutationMap
   *out << Verbose(1) << "Argument minimising the PermutationMap, at current potential " << OldPotential << " ... " << endl;
@@ -306,10 +384,10 @@
       while (Walker->next != end) { // pick one
         Walker = Walker->next;
-        PrintPermutationMap(out, PermutationMap, AtomCount);
-        Sprinter = DistanceIterators[Walker->nr]->second;   // store initial partner
-        Strider = DistanceIterators[Walker->nr];  //remember old iterator
-        DistanceIterators[Walker->nr] = StepList[Walker->nr];
-        if (DistanceIterators[Walker->nr] == DistanceList[Walker->nr]->end()) {// stop, before we run through the list and still on
-          DistanceIterators[Walker->nr] == DistanceList[Walker->nr]->begin();
+        PrintPermutationMap(out, AtomCount, Params);
+        Sprinter = Params.DistanceIterators[Walker->nr]->second;   // store initial partner
+        Strider = Params.DistanceIterators[Walker->nr];  //remember old iterator
+        Params.DistanceIterators[Walker->nr] = Params.StepList[Walker->nr];
+        if (Params.DistanceIterators[Walker->nr] == Params.DistanceList[Walker->nr]->end()) {// stop, before we run through the list and still on
+          Params.DistanceIterators[Walker->nr] == Params.DistanceList[Walker->nr]->begin();
           break;
         }
@@ -318,5 +396,5 @@
         Runner = start->next;
         while(Runner != end) { // find the source whose toes we might be stepping on (Walker's new target should be in use by another already)
-          if (PermutationMap[Runner->nr] == DistanceIterators[Walker->nr]->second) {
+          if (Params.PermutationMap[Runner->nr] == Params.DistanceIterators[Walker->nr]->second) {
             //*out << Verbose(2) << "Found the corresponding owner " << *Runner << " to " << *PermutationMap[Runner->nr] << "." << endl;
             break;
@@ -326,32 +404,32 @@
         if (Runner != end) { // we found the other source
           // then look in its distance list for Sprinter
-          Rider = DistanceList[Runner->nr]->begin();
-          for (; Rider != DistanceList[Runner->nr]->end(); Rider++)
+          Rider = Params.DistanceList[Runner->nr]->begin();
+          for (; Rider != Params.DistanceList[Runner->nr]->end(); Rider++)
             if (Rider->second == Sprinter)
               break;
-          if (Rider != DistanceList[Runner->nr]->end()) { // if we have found one
+          if (Rider != Params.DistanceList[Runner->nr]->end()) { // if we have found one
             //*out << Verbose(2) << "Current Other: " << *Runner << " with old/next candidate " << *PermutationMap[Runner->nr] << "/" << *Rider->second << "." << endl;
             // exchange both
-            PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second; // put next farther distance into PermutationMap
-            PermutationMap[Runner->nr] = Sprinter;  // and hand the old target to its respective owner
-            PrintPermutationMap(out, PermutationMap, AtomCount);
+            Params.PermutationMap[Walker->nr] = Params.DistanceIterators[Walker->nr]->second; // put next farther distance into PermutationMap
+            Params.PermutationMap[Runner->nr] = Sprinter;  // and hand the old target to its respective owner
+            PrintPermutationMap(out, AtomCount, Params);
             // calculate the new potential
             //*out << Verbose(2) << "Checking new potential ..." << endl;
-            Potential = ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem);
+            Potential = ConstrainedPotential(out, Params);
             if (Potential > OldPotential) { // we made everything worse! Undo ...
               //*out << Verbose(3) << "Nay, made the potential worse: " << Potential << " vs. " << OldPotential << "!" << endl;
-              //*out << Verbose(3) << "Setting " << *Runner << "'s source to " << *DistanceIterators[Runner->nr]->second << "." << endl;
+              //*out << Verbose(3) << "Setting " << *Runner << "'s source to " << *Params.DistanceIterators[Runner->nr]->second << "." << endl;
               // Undo for Runner (note, we haven't moved the iteration yet, we may use this)
-              PermutationMap[Runner->nr] = DistanceIterators[Runner->nr]->second;
+              Params.PermutationMap[Runner->nr] = Params.DistanceIterators[Runner->nr]->second;
               // Undo for Walker
-              DistanceIterators[Walker->nr] = Strider;  // take next farther distance target
-              //*out << Verbose(3) << "Setting " << *Walker << "'s source to " << *DistanceIterators[Walker->nr]->second << "." << endl;
-              PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second;
+              Params.DistanceIterators[Walker->nr] = Strider;  // take next farther distance target
+              //*out << Verbose(3) << "Setting " << *Walker << "'s source to " << *Params.DistanceIterators[Walker->nr]->second << "." << endl;
+              Params.PermutationMap[Walker->nr] = Params.DistanceIterators[Walker->nr]->second;
             } else {
-              DistanceIterators[Runner->nr] = Rider;  // if successful also move the pointer in the iterator list
+              Params.DistanceIterators[Runner->nr] = Rider;  // if successful also move the pointer in the iterator list
               *out << Verbose(3) << "Found a better permutation, new potential is " << Potential << " vs." << OldPotential << "." << endl;
               OldPotential = Potential;
             }
-            if (Potential > constants[2]) {
+            if (Potential > Params.PenaltyConstants[2]) {
               cerr << "ERROR: The two-step permutation procedure did not maintain injectivity!" << endl;
               exit(255);
@@ -363,7 +441,7 @@
           }
         } else {
-          PermutationMap[Walker->nr] = DistanceIterators[Walker->nr]->second; // new target has no source!
+          Params.PermutationMap[Walker->nr] = Params.DistanceIterators[Walker->nr]->second; // new target has no source!
         }
-        StepList[Walker->nr]++; // take next farther distance target
+        Params.StepList[Walker->nr]++; // take next farther distance target
       }
     } while (Walker->next != end);
@@ -374,9 +452,10 @@
   /// free memory and return with evaluated potential
   for (int i=AtomCount; i--;)
-    DistanceList[i]->clear();
-  Free(&DistanceList);
-  Free(&DistanceIterators);
-  return ConstrainedPotential(out, PermutationMap, startstep, endstep, constants, IsAngstroem);
-};
+    Params.DistanceList[i]->clear();
+  Free(&Params.DistanceList);
+  Free(&Params.DistanceIterators);
+  return ConstrainedPotential(out, Params);
+};
+
 
 /** Evaluates the (distance-related part) of the constrained potential for the constrained forces.
@@ -390,17 +469,7 @@
 void molecule::EvaluateConstrainedForces(ofstream *out, int startstep, int endstep, atom **PermutationMap, ForceMatrix *Force)
 {
-  double constant = 10.;
-  atom *Walker = NULL, *Sprinter = NULL;
-
   /// evaluate forces (only the distance to target dependent part) with the final PermutationMap
   *out << Verbose(1) << "Calculating forces and adding onto ForceMatrix ... " << endl;
-  Walker = start;
-  while (Walker->next != NULL) {
-    Walker = Walker->next;
-    Sprinter = PermutationMap[Walker->nr];
-    // set forces
-    for (int i=NDIM;i++;)
-      Force->Matrix[0][Walker->nr][5+i] += 2.*constant*sqrt(Walker->Trajectory.R.at(startstep).Distance(&Sprinter->Trajectory.R.at(endstep)));
-  }
+  ActOnAllAtoms( &atom::EvaluateConstrainedForce, startstep, endstep, PermutationMap, Force );
   *out << Verbose(1) << "done." << endl;
 };
