Index: src/Dynamics/MinimiseConstrainedPotential.cpp
===================================================================
--- src/Dynamics/MinimiseConstrainedPotential.cpp	(revision 9e23a3785b6bdd916d370ba9157e9b76a1905c53)
+++ src/Dynamics/MinimiseConstrainedPotential.cpp	(revision 9e23a3785b6bdd916d370ba9157e9b76a1905c53)
@@ -0,0 +1,411 @@
+/*
+ * Project: MoleCuilder
+ * Description: creates and alters molecular systems
+ * Copyright (C)  2010 University of Bonn. All rights reserved.
+ * Please see the LICENSE file or "Copyright notice" in builder.cpp for details.
+ */
+
+/*
+ * MinimiseConstrainedPotential.cpp
+ *
+ *  Created on: Feb 23, 2011
+ *      Author: heber
+ */
+
+// include config.h
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include "CodePatterns/MemDebug.hpp"
+
+#include <gsl/gsl_matrix.h>
+#include <gsl/gsl_vector.h>
+#include <gsl/gsl_linalg.h>
+
+#include "atom.hpp"
+#include "config.hpp"
+#include "element.hpp"
+#include "CodePatterns/enumeration.hpp"
+#include "CodePatterns/Info.hpp"
+#include "CodePatterns/Verbose.hpp"
+#include "CodePatterns/Log.hpp"
+#include "molecule.hpp"
+#include "parser.hpp"
+#include "LinearAlgebra/Plane.hpp"
+#include "World.hpp"
+
+#include "Dynamics/MinimiseConstrainedPotential.hpp"
+
+
+MinimiseConstrainedPotential::MinimiseConstrainedPotential(molecule::atomSet &_atoms) :
+  atoms(_atoms)
+{}
+
+MinimiseConstrainedPotential::~MinimiseConstrainedPotential()
+{}
+
+double MinimiseConstrainedPotential::operator()(atom **&PermutationMap, int _startstep, int _endstep, bool IsAngstroem)
+{
+  double Potential, OldPotential, OlderPotential;
+  PermutationMap = new atom *[atoms.size()];
+  DistanceList = new DistanceMap *[atoms.size()];
+  DistanceIterators = new DistanceMap::iterator[atoms.size()];
+  DoubleList = new int[atoms.size()];
+  StepList = new DistanceMap::iterator[atoms.size()];
+  int round;
+  atom *Sprinter = NULL;
+  DistanceMap::iterator Rider, Strider;
+
+  // set to zero
+  for (size_t i=0;i<atoms.size();i++) {
+    PermutationMap[i] = NULL;
+    DoubleList[i] = 0;
+  }
+
+  /// Minimise the potential
+  // set Lagrange multiplier constants
+  PenaltyConstants[0] = 10.;
+  PenaltyConstants[1] = 1.;
+  PenaltyConstants[2] = 1e+7;    // just a huge penalty
+  // generate the distance list
+  DoLog(1) && (Log() << Verbose(1) << "Allocating, initializting and filling the distance list ... " << endl);
+  FillDistanceList();
+
+  // create the initial PermutationMap (source -> target)
+  CreateInitialLists();
+
+  // make the PermutationMap injective by checking whether we have a non-zero constants[2] term in it
+  DoLog(1) && (Log() << Verbose(1) << "Making the PermutationMap injective ... " << endl);
+  MakeInjectivePermutation();
+  delete[](DoubleList);
+
+  // argument minimise the constrained potential in this injective PermutationMap
+  DoLog(1) && (Log() << Verbose(1) << "Argument minimising the PermutationMap." << endl);
+  OldPotential = 1e+10;
+  round = 0;
+  do {
+    DoLog(2) && (Log() << Verbose(2) << "Starting round " << ++round << ", at current potential " << OldPotential << " ... " << endl);
+    OlderPotential = OldPotential;
+    molecule::atomSet::const_iterator iter;
+    do {
+      iter = atoms.begin();
+      for (; iter != atoms.end(); ++iter) {
+        PrintPermutationMap();
+        Sprinter = DistanceIterators[(*iter)->getNr()]->second;   // store initial partner
+        Strider = DistanceIterators[(*iter)->getNr()];  //remember old iterator
+        DistanceIterators[(*iter)->getNr()] = StepList[(*iter)->getNr()];
+        if (DistanceIterators[(*iter)->getNr()] == DistanceList[(*iter)->getNr()]->end()) {// stop, before we run through the list and still on
+          DistanceIterators[(*iter)->getNr()] == DistanceList[(*iter)->getNr()]->begin();
+          break;
+        }
+        //Log() << Verbose(2) << "Current Walker: " << *(*iter) << " with old/next candidate " << *Sprinter << "/" << *DistanceIterators[(*iter)->getNr()]->second << "." << endl;
+        // find source of the new target
+        molecule::atomSet::const_iterator runner = atoms.begin();
+        for (; runner != atoms.end(); ++runner) { // find the source whose toes we might be stepping on (Walker's new target should be in use by another already)
+          if (PermutationMap[(*runner)->getNr()] == DistanceIterators[(*iter)->getNr()]->second) {
+            //Log() << Verbose(2) << "Found the corresponding owner " << *(*runner) << " to " << *PermutationMap[(*runner)->getNr()] << "." << endl;
+            break;
+          }
+        }
+        if (runner != atoms.end()) { // we found the other source
+          // then look in its distance list for Sprinter
+          Rider = DistanceList[(*runner)->getNr()]->begin();
+          for (; Rider != DistanceList[(*runner)->getNr()]->end(); Rider++)
+            if (Rider->second == Sprinter)
+              break;
+          if (Rider != DistanceList[(*runner)->getNr()]->end()) { // if we have found one
+            //Log() << Verbose(2) << "Current Other: " << *(*runner) << " with old/next candidate " << *PermutationMap[(*runner)->getNr()] << "/" << *Rider->second << "." << endl;
+            // exchange both
+            PermutationMap[(*iter)->getNr()] = DistanceIterators[(*iter)->getNr()]->second; // put next farther distance into PermutationMap
+            PermutationMap[(*runner)->getNr()] = Sprinter;  // and hand the old target to its respective owner
+            PrintPermutationMap();
+            // calculate the new potential
+            //Log() << Verbose(2) << "Checking new potential ..." << endl;
+            Potential = ConstrainedPotential();
+            if (Potential > OldPotential) { // we made everything worse! Undo ...
+              //Log() << Verbose(3) << "Nay, made the potential worse: " << Potential << " vs. " << OldPotential << "!" << endl;
+              //Log() << Verbose(3) << "Setting " << *(*runner) << "'s source to " << *DistanceIterators[(*runner)->getNr()]->second << "." << endl;
+              // Undo for Runner (note, we haven't moved the iteration yet, we may use this)
+              PermutationMap[(*runner)->getNr()] = DistanceIterators[(*runner)->getNr()]->second;
+              // Undo for Walker
+              DistanceIterators[(*iter)->getNr()] = Strider;  // take next farther distance target
+              //Log() << Verbose(3) << "Setting " << *(*iter) << "'s source to " << *DistanceIterators[(*iter)->getNr()]->second << "." << endl;
+              PermutationMap[(*iter)->getNr()] = DistanceIterators[(*iter)->getNr()]->second;
+            } else {
+              DistanceIterators[(*runner)->getNr()] = Rider;  // if successful also move the pointer in the iterator list
+              DoLog(3) && (Log() << Verbose(3) << "Found a better permutation, new potential is " << Potential << " vs." << OldPotential << "." << endl);
+              OldPotential = Potential;
+            }
+            if (Potential > PenaltyConstants[2]) {
+              DoeLog(1) && (eLog()<< Verbose(1) << "The two-step permutation procedure did not maintain injectivity!" << endl);
+              exit(255);
+            }
+            //Log() << Verbose(0) << endl;
+          } else {
+            DoeLog(1) && (eLog()<< Verbose(1) << **runner << " was not the owner of " << *Sprinter << "!" << endl);
+            exit(255);
+          }
+        } else {
+          PermutationMap[(*iter)->getNr()] = DistanceIterators[(*iter)->getNr()]->second; // new target has no source!
+        }
+        StepList[(*iter)->getNr()]++; // take next farther distance target
+      }
+    } while (++iter != atoms.end());
+  } while ((OlderPotential - OldPotential) > 1e-3);
+  DoLog(1) && (Log() << Verbose(1) << "done." << endl);
+
+
+  /// free memory and return with evaluated potential
+  for (int i=atoms.size(); i--;)
+    DistanceList[i]->clear();
+  delete[](DistanceList);
+  delete[](DistanceIterators);
+  return ConstrainedPotential();
+};
+
+void MinimiseConstrainedPotential::FillDistanceList()
+{
+  for (int i=atoms.size(); i--;) {
+    DistanceList[i] = new DistanceMap;    // is the distance sorted target list per atom
+    DistanceList[i]->clear();
+  }
+
+  for (molecule::atomSet::const_iterator iter = atoms.begin(); iter != atoms.end(); ++iter) {
+    for (molecule::atomSet::const_iterator runner = atoms.begin(); runner != atoms.end(); ++runner) {
+      DistanceList[(*iter)->getNr()]->insert( DistancePair((*iter)->getPositionAtStep(startstep).distance((*runner)->getPositionAtStep(endstep)), (*runner)) );
+    }
+  }
+};
+
+void MinimiseConstrainedPotential::CreateInitialLists()
+{
+  for (molecule::atomSet::const_iterator iter = atoms.begin(); iter != atoms.end(); ++iter) {
+    StepList[(*iter)->getNr()] = DistanceList[(*iter)->getNr()]->begin();    // stores the step to the next iterator that could be a possible next target
+    PermutationMap[(*iter)->getNr()] = DistanceList[(*iter)->getNr()]->begin()->second;   // always pick target with the smallest distance
+    DoubleList[DistanceList[(*iter)->getNr()]->begin()->second->getNr()]++;            // increase this target's source count (>1? not injective)
+    DistanceIterators[(*iter)->getNr()] = DistanceList[(*iter)->getNr()]->begin();    // and remember which one we picked
+    DoLog(2) && (Log() << Verbose(2) << **iter << " starts with distance " << DistanceList[(*iter)->getNr()]->begin()->first << "." << endl);
+  }
+};
+
+void MinimiseConstrainedPotential::MakeInjectivePermutation()
+{
+  molecule::atomSet::const_iterator iter = atoms.begin();
+  DistanceMap::iterator NewBase;
+  double Potential = fabs(ConstrainedPotential());
+
+  if (atoms.empty()) {
+    eLog() << Verbose(1) << "Molecule is empty." << endl;
+    return;
+  }
+  while ((Potential) > PenaltyConstants[2]) {
+    PrintPermutationMap();
+    iter++;
+    if (iter == atoms.end()) // round-robin at the end
+      iter = atoms.begin();
+    if (DoubleList[DistanceIterators[(*iter)->getNr()]->second->getNr()] <= 1)  // no need to make those injective that aren't
+      continue;
+    // now, try finding a new one
+    Potential = TryNextNearestNeighbourForInjectivePermutation((*iter), Potential);
+  }
+  for (int i=atoms.size(); i--;) // now each single entry in the DoubleList should be <=1
+    if (DoubleList[i] > 1) {
+      DoeLog(0) && (eLog()<< Verbose(0) << "Failed to create an injective PermutationMap!" << endl);
+      performCriticalExit();
+    }
+  DoLog(1) && (Log() << Verbose(1) << "done." << endl);
+};
+
+void MinimiseConstrainedPotential::PrintPermutationMap() const
+{
+  stringstream zeile1, zeile2;
+  const int AtomCount = atoms.size();
+  int *DoubleList = new int[AtomCount];
+  for(int i=0;i<AtomCount;i++)
+    DoubleList[i] = 0;
+  int doubles = 0;
+  zeile1 << "PermutationMap: ";
+  zeile2 << "                ";
+  for (int i=0;i<AtomCount;i++) {
+    DoubleList[PermutationMap[i]->getNr()]++;
+    zeile1 << i << " ";
+    zeile2 << PermutationMap[i]->getNr() << " ";
+  }
+  for (int i=0;i<AtomCount;i++)
+    if (DoubleList[i] > 1)
+    doubles++;
+  if (doubles >0)
+    DoLog(2) && (Log() << Verbose(2) << "Found " << doubles << " Doubles." << endl);
+  delete[](DoubleList);
+//  Log() << Verbose(2) << zeile1.str() << endl << zeile2.str() << endl;
+};
+
+double MinimiseConstrainedPotential::ConstrainedPotential()
+{
+  double tmp = 0.;
+  double result = 0.;
+  // go through every atom
+  atom *Runner = NULL;
+  for (molecule::atomSet::const_iterator iter = atoms.begin(); iter != atoms.end(); ++iter) {
+    // first term: distance to target
+    Runner = PermutationMap[(*iter)->getNr()];   // find target point
+    tmp = ((*iter)->getPositionAtStep(startstep).distance(Runner->getPositionAtStep(endstep)));
+    tmp *= IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
+    result += PenaltyConstants[0] * tmp;
+    //Log() << Verbose(4) << "Adding " << tmp*constants[0] << "." << endl;
+
+    // second term: sum of distances to other trajectories
+    result += SumDistanceOfTrajectories((*iter));
+
+    // third term: penalty for equal targets
+    result += PenalizeEqualTargets((*iter));
+  }
+
+  return result;
+};
+
+double MinimiseConstrainedPotential::TryNextNearestNeighbourForInjectivePermutation(atom *Walker, double &OldPotential)
+{
+  double Potential = 0;
+  DistanceMap::iterator NewBase = DistanceIterators[Walker->getNr()];  // 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->getNr()] != 0) && (NewBase != DistanceList[Walker->getNr()]->end()));
+  if (NewBase != DistanceList[Walker->getNr()]->end()) {
+    PermutationMap[Walker->getNr()] = NewBase->second;
+    Potential = fabs(ConstrainedPotential());
+    if (Potential > OldPotential) { // undo
+      PermutationMap[Walker->getNr()] = DistanceIterators[Walker->getNr()]->second;
+    } else {  // do
+      DoubleList[DistanceIterators[Walker->getNr()]->second->getNr()]--;  // decrease the old entry in the doubles list
+      DoubleList[NewBase->second->getNr()]++;    // increase the old entry in the doubles list
+      DistanceIterators[Walker->getNr()] = NewBase;
+      OldPotential = Potential;
+      DoLog(3) && (Log() << Verbose(3) << "Found a new permutation, new potential is " << OldPotential << "." << endl);
+    }
+  }
+  return Potential;
+};
+
+double MinimiseConstrainedPotential::PenalizeEqualTargets(atom *Walker)
+{
+  double result = 0.;
+  for (molecule::atomSet::const_iterator iter = atoms.begin(); iter != atoms.end(); ++iter) {
+    if ((PermutationMap[Walker->getNr()] == PermutationMap[(*iter)->getNr()]) && (Walker->getNr() < (*iter)->getNr())) {
+  //    atom *Sprinter = PermutationMap[Walker->nr];
+  //        Log() << Verbose(0) << *Walker << " and " << *(*iter) << " are heading to the same target at ";
+  //        Log() << Verbose(0) << Sprinter->getPosition(endstep);
+  //        Log() << Verbose(0) << ", penalting." << endl;
+      result += PenaltyConstants[2];
+      //Log() << Verbose(4) << "Adding " << constants[2] << "." << endl;
+    }
+  }
+  return result;
+};
+
+double MinimiseConstrainedPotential::SumDistanceOfTrajectories(atom *Walker)
+{
+  gsl_matrix *A = gsl_matrix_alloc(NDIM,NDIM);
+  gsl_vector *x = gsl_vector_alloc(NDIM);
+  atom *Sprinter = NULL;
+  Vector trajectory1, trajectory2, normal, TestVector;
+  double Norm1, Norm2, tmp, result = 0.;
+
+  for (molecule::atomSet::const_iterator iter = atoms.begin(); iter != atoms.end(); ++iter) {
+    if ((*iter) == 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->getNr()];   // find first target point
+    trajectory1 = Sprinter->getPositionAtStep(endstep) - Walker->getPositionAtStep(startstep);
+    trajectory1.Normalize();
+    Norm1 = trajectory1.Norm();
+    Sprinter = PermutationMap[(*iter)->getNr()];   // find second target point
+    trajectory2 = Sprinter->getPositionAtStep(endstep) - (*iter)->getPositionAtStep(startstep);
+    trajectory2.Normalize();
+    Norm2 = trajectory1.Norm();
+    // check whether either is zero()
+    if ((Norm1 < MYEPSILON) && (Norm2 < MYEPSILON)) {
+      tmp = Walker->getPositionAtStep(startstep).distance((*iter)->getPositionAtStep(startstep));
+    } else if (Norm1 < MYEPSILON) {
+      Sprinter = PermutationMap[Walker->getNr()];   // find first target point
+      trajectory1 = Sprinter->getPositionAtStep(endstep) - (*iter)->getPositionAtStep(startstep);
+      trajectory2 *= trajectory1.ScalarProduct(trajectory2); // trajectory2 is scaled to unity, hence we don't need to divide by anything
+      trajectory1 -= trajectory2;   // project the part in norm direction away
+      tmp = trajectory1.Norm();  // remaining norm is distance
+    } else if (Norm2 < MYEPSILON) {
+      Sprinter = PermutationMap[(*iter)->getNr()];   // find second target point
+      trajectory2 = Sprinter->getPositionAtStep(endstep) - Walker->getPositionAtStep(startstep);  // copy second offset
+      trajectory1 *= trajectory2.ScalarProduct(trajectory1); // trajectory1 is scaled to unity, hence we don't need to divide by anything
+      trajectory2 -= 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
+  //        Log() << Verbose(3) << "Both trajectories of " << *Walker << " and " << *Runner << " are linear dependent: ";
+  //        Log() << Verbose(0) << trajectory1;
+  //        Log() << Verbose(0) << " and ";
+  //        Log() << Verbose(0) << trajectory2;
+      tmp = Walker->getPositionAtStep(startstep).distance((*iter)->getPositionAtStep(startstep));
+  //        Log() << Verbose(0) << " with distance " << tmp << "." << endl;
+    } else { // determine distance by finding minimum distance
+  //        Log() << Verbose(3) << "Both trajectories of " << *Walker << " and " << *(*iter) << " are linear independent ";
+  //        Log() << Verbose(0) << endl;
+  //        Log() << Verbose(0) << "First Trajectory: ";
+  //        Log() << Verbose(0) << trajectory1 << endl;
+  //        Log() << Verbose(0) << "Second Trajectory: ";
+  //        Log() << Verbose(0) << trajectory2 << endl;
+      // determine normal vector for both
+      normal = Plane(trajectory1, trajectory2,0).getNormal();
+      // print all vectors for debugging
+  //        Log() << Verbose(0) << "Normal vector in between: ";
+  //        Log() << Verbose(0) << normal << endl;
+      // setup matrix
+      for (int i=NDIM;i--;) {
+        gsl_matrix_set(A, 0, i, trajectory1[i]);
+        gsl_matrix_set(A, 1, i, trajectory2[i]);
+        gsl_matrix_set(A, 2, i, normal[i]);
+        gsl_vector_set(x,i, (Walker->getPositionAtStep(startstep)[i] - (*iter)->getPositionAtStep(startstep)[i]));
+      }
+      // solve the linear system by Householder transformations
+      gsl_linalg_HH_svx(A, x);
+      // distance from last component
+      tmp = gsl_vector_get(x,2);
+  //        Log() << Verbose(0) << " with distance " << tmp << "." << endl;
+      // test whether we really have the intersection (by checking on c_1 and c_2)
+      trajectory1.Scale(gsl_vector_get(x,0));
+      trajectory2.Scale(gsl_vector_get(x,1));
+      normal.Scale(gsl_vector_get(x,2));
+      TestVector = (*iter)->getPositionAtStep(startstep) + trajectory2 + normal
+                   - (Walker->getPositionAtStep(startstep) + trajectory1);
+      if (TestVector.Norm() < MYEPSILON) {
+  //          Log() << Verbose(2) << "Test: ok.\tDistance of " << tmp << " is correct." << endl;
+      } else {
+  //          Log() << Verbose(2) << "Test: failed.\tIntersection is off by ";
+  //          Log() << Verbose(0) << TestVector;
+  //          Log() << Verbose(0) << "." << endl;
+      }
+    }
+    // add up
+    tmp *= IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
+    if (fabs(tmp) > MYEPSILON) {
+      result += PenaltyConstants[1] * 1./tmp;
+      //Log() << Verbose(4) << "Adding " << 1./tmp*constants[1] << "." << endl;
+    }
+  }
+  return result;
+};
+
+void MinimiseConstrainedPotential::EvaluateConstrainedForces(ForceMatrix *Force)
+{
+  double constant = 10.;
+
+  /// evaluate forces (only the distance to target dependent part) with the final PermutationMap
+  DoLog(1) && (Log() << Verbose(1) << "Calculating forces and adding onto ForceMatrix ... " << endl);
+  for(molecule::atomSet::const_iterator iter = atoms.begin(); iter != atoms.end(); ++iter) {
+    atom *Sprinter = PermutationMap[(*iter)->getNr()];
+    // set forces
+    for (int i=NDIM;i++;)
+      Force->Matrix[0][(*iter)->getNr()][5+i] += 2.*constant*sqrt((*iter)->getPositionAtStep(startstep).distance(Sprinter->getPositionAtStep(endstep)));
+  }
+  DoLog(1) && (Log() << Verbose(1) << "done." << endl);
+};
+
Index: src/Dynamics/MinimiseConstrainedPotential.hpp
===================================================================
--- src/Dynamics/MinimiseConstrainedPotential.hpp	(revision 9e23a3785b6bdd916d370ba9157e9b76a1905c53)
+++ src/Dynamics/MinimiseConstrainedPotential.hpp	(revision 9e23a3785b6bdd916d370ba9157e9b76a1905c53)
@@ -0,0 +1,159 @@
+/*
+ * MinimiseConstrainedPotential.hpp
+ *
+ *  Created on: Feb 23, 2011
+ *      Author: heber
+ */
+
+#ifndef MINIMISECONSTRAINEDPOTENTIAL_HPP_
+#define MINIMISECONSTRAINEDPOTENTIAL_HPP_
+
+// include config.h
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+class atom;
+
+#include <vector>
+#include <map>
+
+#include "molecule.hpp"
+
+/** Structure to contain parameters needed for evaluation of constraint potential.
+ *
+ */
+class MinimiseConstrainedPotential
+{
+public:
+  MinimiseConstrainedPotential(molecule::atomSet &_atoms);
+  ~MinimiseConstrainedPotential();
+
+  /** Minimises the extra potential for constrained molecular dynamics and gives forces and the constrained potential energy.
+   * We do the following:
+   *  -# Generate a distance list from all source to all target points
+   *  -# Sort this per source point
+   *  -# Take for each source point the target point with minimum distance, use this as initial permutation
+   *  -# check whether molecule::ConstrainedPotential() is greater than injective penalty
+   *     -# If so, we go through each source point, stepping down in the sorted target point distance list and re-checking potential.
+   *  -# Next, we only apply transformations that keep the injectivity of the permutations list.
+   *  -# Hence, for one source point we step down the ladder and seek the corresponding owner of this new target
+   *     point and try to change it for one with lesser distance, or for the next one with greater distance, but only
+   *     if this decreases the conditional potential.
+   *  -# finished.
+   *  -# 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 **PermutationMap on return: mapping between the atom label of the initial and the final configuration
+   * \param startstep current MD step giving initial position between which and \a endstep we perform the constrained MD (as further steps are always concatenated)
+   * \param endstep step giving final position in constrained MD
+   * \param IsAngstroem whether coordinates are in angstroem (true) or bohrradius (false)
+   * \sa molecule::VerletForceIntegration()
+   * \return potential energy (and allocated **PermutationMap (array of molecule::AtomCount ^2)
+   * \todo The constrained potential's constants are set to fixed values right now, but they should scale based on checks of the system in order
+   *       to ensure they're properties (e.g. constants[2] always greater than the energy of the system).
+   * \bug this all is not O(N log N) but O(N^2)
+   */
+  double operator()(atom **&PermutationMap, int startstep, int endstep, bool IsAngstroem);
+
+  /** Evaluates the (distance-related part) of the constrained potential for the constrained forces.
+   * \param *out output stream for debugging
+   * \param startstep current MD step giving initial position between which and \a endstep we perform the constrained MD (as further steps are always concatenated)
+   * \param endstep step giving final position in constrained MD
+   * \param **PermutationMap mapping between the atom label of the initial and the final configuration
+   * \param *Force ForceMatrix containing force vectors from the external energy functional minimisation.
+   * \todo the constant for the constrained potential distance part is hard-coded independently of the hard-coded value in MinimiseConstrainedPotential()
+   */
+  void EvaluateConstrainedForces(ForceMatrix *Force);
+
+private:
+  typedef std::pair < double, atom* > DistancePair;
+  typedef std::multimap < double, atom* > DistanceMap;
+  typedef std::pair < DistanceMap::iterator, bool> DistanceTestPair;
+
+  molecule::atomSet atoms;
+  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
+
+  /** \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();
+
+  /** Initialize lists.
+   * \param *out output stream for debugging
+   * \param *mol molecule to scan distances in
+   * \param &Params constrained potential parameters
+   */
+  void CreateInitialLists();
+
+  /** 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();
+
+  /** Print the current permutation map.
+   * \param *out output stream for debugging
+   * \param &Params constrained potential parameters
+   * \param AtomCount number of atoms
+   */
+  void PrintPermutationMap() const;
+
+  /** 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 \frac{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}\f$ 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 &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 ConstrainedPotential();
+
+  /** 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(atom *Walker, double &OldPotential);
+
+  /** 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);
+
+  /** 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);
+
+};
+
+
+#endif /* MINIMISECONSTRAINEDPOTENTIAL_HPP_ */
Index: src/Makefile.am
===================================================================
--- src/Makefile.am	(revision 632508ffb01a2e25577835bce4302a652209e479)
+++ src/Makefile.am	(revision 9e23a3785b6bdd916d370ba9157e9b76a1905c53)
@@ -119,4 +119,10 @@
   Descriptors/MoleculeSelectionDescriptor_impl.hpp
 
+DYNAMICSSOURCE = \
+	Dynamics/MinimiseConstrainedPotential.cpp
+
+DYNAMICSHEADER = \
+	Dynamics/MinimiseConstrainedPotential.hpp
+
 GRAPHSOURCE = \
   Graph/BondGraph.cpp
@@ -170,4 +176,5 @@
   ${ATOMSOURCE} \
   ${DESCRIPTORSOURCE} \
+  ${DYNAMICSSOURCE} \
   ${GRAPHSOURCE} \
   ${RANDOMSOURCE} \
@@ -207,4 +214,5 @@
   ${DESCRIPTORHEADER} \
   ${DESCRIPTORIMPLHEADER} \
+  ${DYNAMICSHEADER} \
   ${GRAPHHEADER} \
   ${RANDOMSOURCE} \
Index: src/molecule.hpp
===================================================================
--- src/molecule.hpp	(revision 632508ffb01a2e25577835bce4302a652209e479)
+++ src/molecule.hpp	(revision 9e23a3785b6bdd916d370ba9157e9b76a1905c53)
@@ -55,25 +55,5 @@
 #define MoleculeListTest pair <MoleculeList::iterator, bool>
 
-#define DistancePair pair < double, atom* >
-#define DistanceMap multimap < double, atom* >
-#define DistanceTestPair pair < DistanceMap::iterator, bool>
-
-
 /************************************* 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
-};
 
 /** The complete molecule.
@@ -201,7 +181,4 @@
   void RotateToPrincipalAxisSystem(Vector &Axis);
 
-  double ConstrainedPotential(struct EvaluatePotential &Params);
-  double MinimiseConstrainedPotential(atom **&permutation, int startstep, int endstep, bool IsAngstroem);
-  void EvaluateConstrainedForces(int startstep, int endstep, atom **PermutationMap, ForceMatrix *Force);
   bool LinearInterpolationBetweenConfiguration(int startstep, int endstep, std::string prefix, config &configuration, bool MapByIdentity);
 
Index: src/molecule_dynamics.cpp
===================================================================
--- src/molecule_dynamics.cpp	(revision 632508ffb01a2e25577835bce4302a652209e479)
+++ src/molecule_dynamics.cpp	(revision 9e23a3785b6bdd916d370ba9157e9b76a1905c53)
@@ -23,11 +23,12 @@
 #include "atom.hpp"
 #include "config.hpp"
-#include "element.hpp"
+#include "Dynamics/MinimiseConstrainedPotential.hpp"
+//#include "element.hpp"
 #include "CodePatterns/Info.hpp"
-#include "CodePatterns/Verbose.hpp"
-#include "CodePatterns/Log.hpp"
+//#include "CodePatterns/Verbose.hpp"
+//#include "CodePatterns/Log.hpp"
 #include "molecule.hpp"
 #include "parser.hpp"
-#include "LinearAlgebra/Plane.hpp"
+//#include "LinearAlgebra/Plane.hpp"
 #include "ThermoStatContainer.hpp"
 #include "Thermostats/Berendsen.hpp"
@@ -35,462 +36,6 @@
 #include "CodePatterns/enumeration.hpp"
 
-#include <gsl/gsl_matrix.h>
-#include <gsl/gsl_vector.h>
-#include <gsl/gsl_linalg.h>
-
 /************************************* 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 *Sprinter = NULL;
-  Vector trajectory1, trajectory2, normal, TestVector;
-  double Norm1, Norm2, tmp, result = 0.;
-
-  for (molecule::const_iterator iter = mol->begin(); iter != mol->end(); ++iter) {
-    if ((*iter) == 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->getNr()];   // find first target point
-    trajectory1 = Sprinter->getPositionAtStep(Params.endstep) - Walker->getPositionAtStep(Params.startstep);
-    trajectory1.Normalize();
-    Norm1 = trajectory1.Norm();
-    Sprinter = Params.PermutationMap[(*iter)->getNr()];   // find second target point
-    trajectory2 = Sprinter->getPositionAtStep(Params.endstep) - (*iter)->getPositionAtStep(Params.startstep);
-    trajectory2.Normalize();
-    Norm2 = trajectory1.Norm();
-    // check whether either is zero()
-    if ((Norm1 < MYEPSILON) && (Norm2 < MYEPSILON)) {
-      tmp = Walker->getPositionAtStep(Params.startstep).distance((*iter)->getPositionAtStep(Params.startstep));
-    } else if (Norm1 < MYEPSILON) {
-      Sprinter = Params.PermutationMap[Walker->getNr()];   // find first target point
-      trajectory1 = Sprinter->getPositionAtStep(Params.endstep) - (*iter)->getPositionAtStep(Params.startstep);
-      trajectory2 *= trajectory1.ScalarProduct(trajectory2); // trajectory2 is scaled to unity, hence we don't need to divide by anything
-      trajectory1 -= trajectory2;   // project the part in norm direction away
-      tmp = trajectory1.Norm();  // remaining norm is distance
-    } else if (Norm2 < MYEPSILON) {
-      Sprinter = Params.PermutationMap[(*iter)->getNr()];   // find second target point
-      trajectory2 = Sprinter->getPositionAtStep(Params.endstep) - Walker->getPositionAtStep(Params.startstep);  // copy second offset
-      trajectory1 *= trajectory2.ScalarProduct(trajectory1); // trajectory1 is scaled to unity, hence we don't need to divide by anything
-      trajectory2 -= 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
-  //        Log() << Verbose(3) << "Both trajectories of " << *Walker << " and " << *Runner << " are linear dependent: ";
-  //        Log() << Verbose(0) << trajectory1;
-  //        Log() << Verbose(0) << " and ";
-  //        Log() << Verbose(0) << trajectory2;
-      tmp = Walker->getPositionAtStep(Params.startstep).distance((*iter)->getPositionAtStep(Params.startstep));
-  //        Log() << Verbose(0) << " with distance " << tmp << "." << endl;
-    } else { // determine distance by finding minimum distance
-  //        Log() << Verbose(3) << "Both trajectories of " << *Walker << " and " << *(*iter) << " are linear independent ";
-  //        Log() << Verbose(0) << endl;
-  //        Log() << Verbose(0) << "First Trajectory: ";
-  //        Log() << Verbose(0) << trajectory1 << endl;
-  //        Log() << Verbose(0) << "Second Trajectory: ";
-  //        Log() << Verbose(0) << trajectory2 << endl;
-      // determine normal vector for both
-      normal = Plane(trajectory1, trajectory2,0).getNormal();
-      // print all vectors for debugging
-  //        Log() << Verbose(0) << "Normal vector in between: ";
-  //        Log() << Verbose(0) << normal << endl;
-      // setup matrix
-      for (int i=NDIM;i--;) {
-        gsl_matrix_set(A, 0, i, trajectory1[i]);
-        gsl_matrix_set(A, 1, i, trajectory2[i]);
-        gsl_matrix_set(A, 2, i, normal[i]);
-        gsl_vector_set(x,i, (Walker->getPositionAtStep(Params.startstep)[i] - (*iter)->getPositionAtStep(Params.startstep)[i]));
-      }
-      // solve the linear system by Householder transformations
-      gsl_linalg_HH_svx(A, x);
-      // distance from last component
-      tmp = gsl_vector_get(x,2);
-  //        Log() << Verbose(0) << " with distance " << tmp << "." << endl;
-      // test whether we really have the intersection (by checking on c_1 and c_2)
-      trajectory1.Scale(gsl_vector_get(x,0));
-      trajectory2.Scale(gsl_vector_get(x,1));
-      normal.Scale(gsl_vector_get(x,2));
-      TestVector = (*iter)->getPositionAtStep(Params.startstep) + trajectory2 + normal
-                   - (Walker->getPositionAtStep(Params.startstep) + trajectory1);
-      if (TestVector.Norm() < MYEPSILON) {
-  //          Log() << Verbose(2) << "Test: ok.\tDistance of " << tmp << " is correct." << endl;
-      } else {
-  //          Log() << Verbose(2) << "Test: failed.\tIntersection is off by ";
-  //          Log() << Verbose(0) << TestVector;
-  //          Log() << Verbose(0) << "." << endl;
-      }
-    }
-    // add up
-    tmp *= Params.IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
-    if (fabs(tmp) > MYEPSILON) {
-      result += Params.PenaltyConstants[1] * 1./tmp;
-      //Log() << 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.;
-  for (molecule::const_iterator iter = mol->begin(); iter != mol->end(); ++iter) {
-    if ((Params.PermutationMap[Walker->getNr()] == Params.PermutationMap[(*iter)->getNr()]) && (Walker->getNr() < (*iter)->getNr())) {
-  //    atom *Sprinter = PermutationMap[Walker->getNr()];
-  //        Log() << Verbose(0) << *Walker << " and " << *(*iter) << " are heading to the same target at ";
-  //        Log() << Verbose(0) << Sprinter->getPosition(endstep);
-  //        Log() << Verbose(0) << ", penalting." << endl;
-      result += Params.PenaltyConstants[2];
-      //Log() << Verbose(4) << "Adding " << constants[2] << "." << endl;
-    }
-  }
-  return result;
-};
-
-/** 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 \frac{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}\f$ 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 &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(struct EvaluatePotential &Params)
-{
-  double tmp = 0.;
-  double result = 0.;
-  // go through every atom
-  atom *Runner = NULL;
-  for (molecule::const_iterator iter = begin(); iter != end(); ++iter) {
-    // first term: distance to target
-    Runner = Params.PermutationMap[(*iter)->getNr()];   // find target point
-    tmp = ((*iter)->getPositionAtStep(Params.startstep).distance(Runner->getPositionAtStep(Params.endstep)));
-    tmp *= Params.IsAngstroem ? 1. : 1./AtomicLengthToAngstroem;
-    result += Params.PenaltyConstants[0] * tmp;
-    //Log() << Verbose(4) << "Adding " << tmp*constants[0] << "." << endl;
-
-    // second term: sum of distances to other trajectories
-    result += SumDistanceOfTrajectories((*iter), this, Params);
-
-    // third term: penalty for equal targets
-    result += PenalizeEqualTargets((*iter), this, Params);
-  }
-
-  return result;
-};
-
-/** print the current permutation map.
- * \param *out output stream for debugging
- * \param &Params constrained potential parameters
- * \param AtomCount number of atoms
- */
-void PrintPermutationMap(int AtomCount, struct EvaluatePotential &Params)
-{
-  stringstream zeile1, zeile2;
-  int *DoubleList = new int[AtomCount];
-  for(int i=0;i<AtomCount;i++)
-    DoubleList[i] = 0;
-  int doubles = 0;
-  zeile1 << "PermutationMap: ";
-  zeile2 << "                ";
-  for (int i=0;i<AtomCount;i++) {
-    Params.DoubleList[Params.PermutationMap[i]->getNr()]++;
-    zeile1 << i << " ";
-    zeile2 << Params.PermutationMap[i]->getNr() << " ";
-  }
-  for (int i=0;i<AtomCount;i++)
-    if (Params.DoubleList[i] > 1)
-    doubles++;
-  if (doubles >0)
-    DoLog(2) && (Log() << Verbose(2) << "Found " << doubles << " Doubles." << endl);
-  delete[](DoubleList);
-//  Log() << Verbose(2) << 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->getAtomCount(); i--;) {
-    Params.DistanceList[i] = new DistanceMap;    // is the distance sorted target list per atom
-    Params.DistanceList[i]->clear();
-  }
-
-  for (molecule::const_iterator iter = mol->begin(); iter != mol->end(); ++iter) {
-    for (molecule::const_iterator runner = mol->begin(); runner != mol->end(); ++runner) {
-      Params.DistanceList[(*iter)->getNr()]->insert( DistancePair((*iter)->getPositionAtStep(Params.startstep).distance((*runner)->getPositionAtStep(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(molecule *mol, struct EvaluatePotential &Params)
-{
-  for (molecule::const_iterator iter = mol->begin(); iter != mol->end(); ++iter) {
-    Params.StepList[(*iter)->getNr()] = Params.DistanceList[(*iter)->getNr()]->begin();    // stores the step to the next iterator that could be a possible next target
-    Params.PermutationMap[(*iter)->getNr()] = Params.DistanceList[(*iter)->getNr()]->begin()->second;   // always pick target with the smallest distance
-    Params.DoubleList[Params.DistanceList[(*iter)->getNr()]->begin()->second->getNr()]++;            // increase this target's source count (>1? not injective)
-    Params.DistanceIterators[(*iter)->getNr()] = Params.DistanceList[(*iter)->getNr()]->begin();    // and remember which one we picked
-    DoLog(2) && (Log() << Verbose(2) << **iter << " starts with distance " << Params.DistanceList[(*iter)->getNr()]->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(molecule *mol, atom *Walker, double &OldPotential, struct EvaluatePotential &Params)
-{
-  double Potential = 0;
-  DistanceMap::iterator NewBase = Params.DistanceIterators[Walker->getNr()];  // 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->getNr()] != 0) && (NewBase != Params.DistanceList[Walker->getNr()]->end()));
-  if (NewBase != Params.DistanceList[Walker->getNr()]->end()) {
-    Params.PermutationMap[Walker->getNr()] = NewBase->second;
-    Potential = fabs(mol->ConstrainedPotential(Params));
-    if (Potential > OldPotential) { // undo
-      Params.PermutationMap[Walker->getNr()] = Params.DistanceIterators[Walker->getNr()]->second;
-    } else {  // do
-      Params.DoubleList[Params.DistanceIterators[Walker->getNr()]->second->getNr()]--;  // decrease the old entry in the doubles list
-      Params.DoubleList[NewBase->second->getNr()]++;    // increase the old entry in the doubles list
-      Params.DistanceIterators[Walker->getNr()] = NewBase;
-      OldPotential = Potential;
-      DoLog(3) && (Log() << 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(molecule *mol, struct EvaluatePotential &Params)
-{
-  molecule::const_iterator iter = mol->begin();
-  DistanceMap::iterator NewBase;
-  double Potential = fabs(mol->ConstrainedPotential(Params));
-
-  if (mol->empty()) {
-    eLog() << Verbose(1) << "Molecule is empty." << endl;
-    return;
-  }
-  while ((Potential) > Params.PenaltyConstants[2]) {
-    PrintPermutationMap(mol->getAtomCount(), Params);
-    iter++;
-    if (iter == mol->end()) // round-robin at the end
-      iter = mol->begin();
-    if (Params.DoubleList[Params.DistanceIterators[(*iter)->getNr()]->second->getNr()] <= 1)  // no need to make those injective that aren't
-      continue;
-    // now, try finding a new one
-    Potential = TryNextNearestNeighbourForInjectivePermutation(mol, (*iter), Potential, Params);
-  }
-  for (int i=mol->getAtomCount(); i--;) // now each single entry in the DoubleList should be <=1
-    if (Params.DoubleList[i] > 1) {
-      DoeLog(0) && (eLog()<< Verbose(0) << "Failed to create an injective PermutationMap!" << endl);
-      performCriticalExit();
-    }
-  DoLog(1) && (Log() << Verbose(1) << "done." << endl);
-};
-
-/** Minimises the extra potential for constrained molecular dynamics and gives forces and the constrained potential energy.
- * We do the following:
- *  -# Generate a distance list from all source to all target points
- *  -# Sort this per source point
- *  -# Take for each source point the target point with minimum distance, use this as initial permutation
- *  -# check whether molecule::ConstrainedPotential() is greater than injective penalty
- *     -# If so, we go through each source point, stepping down in the sorted target point distance list and re-checking potential.
- *  -# Next, we only apply transformations that keep the injectivity of the permutations list.
- *  -# Hence, for one source point we step down the ladder and seek the corresponding owner of this new target
- *     point and try to change it for one with lesser distance, or for the next one with greater distance, but only
- *     if this decreases the conditional potential.
- *  -# finished.
- *  -# 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 **PermutationMap on return: mapping between the atom label of the initial and the final configuration
- * \param startstep current MD step giving initial position between which and \a endstep we perform the constrained MD (as further steps are always concatenated)
- * \param endstep step giving final position in constrained MD
- * \param IsAngstroem whether coordinates are in angstroem (true) or bohrradius (false)
- * \sa molecule::VerletForceIntegration()
- * \return potential energy (and allocated **PermutationMap (array of molecule::AtomCount ^2)
- * \todo The constrained potential's constants are set to fixed values right now, but they should scale based on checks of the system in order
- *       to ensure they're properties (e.g. constants[2] always greater than the energy of the system).
- * \bug this all is not O(N log N) but O(N^2)
- */
-double molecule::MinimiseConstrainedPotential(atom **&PermutationMap, int startstep, int endstep, bool IsAngstroem)
-{
-  double Potential, OldPotential, OlderPotential;
-  struct EvaluatePotential Params;
-  Params.PermutationMap = new atom *[getAtomCount()];
-  Params.DistanceList = new DistanceMap *[getAtomCount()];
-  Params.DistanceIterators = new DistanceMap::iterator[getAtomCount()];
-  Params.DoubleList = new int[getAtomCount()];
-  Params.StepList = new DistanceMap::iterator[getAtomCount()];
-  int round;
-  atom *Sprinter = NULL;
-  DistanceMap::iterator Rider, Strider;
-
-  // set to zero
-  for (int i=0;i<getAtomCount();i++) {
-    Params.PermutationMap[i] = NULL;
-    Params.DoubleList[i] = 0;
-  }
-
-  /// Minimise the potential
-  // set Lagrange multiplier constants
-  Params.PenaltyConstants[0] = 10.;
-  Params.PenaltyConstants[1] = 1.;
-  Params.PenaltyConstants[2] = 1e+7;    // just a huge penalty
-  // generate the distance list
-  DoLog(1) && (Log() << Verbose(1) << "Allocating, initializting and filling the distance list ... " << endl);
-  FillDistanceList(this, Params);
-
-  // create the initial PermutationMap (source -> target)
-  CreateInitialLists(this, Params);
-
-  // make the PermutationMap injective by checking whether we have a non-zero constants[2] term in it
-  DoLog(1) && (Log() << Verbose(1) << "Making the PermutationMap injective ... " << endl);
-  MakeInjectivePermutation(this, Params);
-  delete[](Params.DoubleList);
-
-  // argument minimise the constrained potential in this injective PermutationMap
-  DoLog(1) && (Log() << Verbose(1) << "Argument minimising the PermutationMap." << endl);
-  OldPotential = 1e+10;
-  round = 0;
-  do {
-    DoLog(2) && (Log() << Verbose(2) << "Starting round " << ++round << ", at current potential " << OldPotential << " ... " << endl);
-    OlderPotential = OldPotential;
-    molecule::const_iterator iter;
-    do {
-      iter = begin();
-      for (; iter != end(); ++iter) {
-        PrintPermutationMap(getAtomCount(), Params);
-        Sprinter = Params.DistanceIterators[(*iter)->getNr()]->second;   // store initial partner
-        Strider = Params.DistanceIterators[(*iter)->getNr()];  //remember old iterator
-        Params.DistanceIterators[(*iter)->getNr()] = Params.StepList[(*iter)->getNr()];
-        if (Params.DistanceIterators[(*iter)->getNr()] == Params.DistanceList[(*iter)->getNr()]->end()) {// stop, before we run through the list and still on
-          Params.DistanceIterators[(*iter)->getNr()] == Params.DistanceList[(*iter)->getNr()]->begin();
-          break;
-        }
-        //Log() << Verbose(2) << "Current Walker: " << *(*iter) << " with old/next candidate " << *Sprinter << "/" << *DistanceIterators[(*iter)->getNr()]->second << "." << endl;
-        // find source of the new target
-        molecule::const_iterator runner = begin();
-        for (; runner != end(); ++runner) { // find the source whose toes we might be stepping on (Walker's new target should be in use by another already)
-          if (Params.PermutationMap[(*runner)->getNr()] == Params.DistanceIterators[(*iter)->getNr()]->second) {
-            //Log() << Verbose(2) << "Found the corresponding owner " << *(*runner) << " to " << *PermutationMap[(*runner)->getNr()] << "." << endl;
-            break;
-          }
-        }
-        if (runner != end()) { // we found the other source
-          // then look in its distance list for Sprinter
-          Rider = Params.DistanceList[(*runner)->getNr()]->begin();
-          for (; Rider != Params.DistanceList[(*runner)->getNr()]->end(); Rider++)
-            if (Rider->second == Sprinter)
-              break;
-          if (Rider != Params.DistanceList[(*runner)->getNr()]->end()) { // if we have found one
-            //Log() << Verbose(2) << "Current Other: " << *(*runner) << " with old/next candidate " << *PermutationMap[(*runner)->getNr()] << "/" << *Rider->second << "." << endl;
-            // exchange both
-            Params.PermutationMap[(*iter)->getNr()] = Params.DistanceIterators[(*iter)->getNr()]->second; // put next farther distance into PermutationMap
-            Params.PermutationMap[(*runner)->getNr()] = Sprinter;  // and hand the old target to its respective owner
-            PrintPermutationMap(getAtomCount(), Params);
-            // calculate the new potential
-            //Log() << Verbose(2) << "Checking new potential ..." << endl;
-            Potential = ConstrainedPotential(Params);
-            if (Potential > OldPotential) { // we made everything worse! Undo ...
-              //Log() << Verbose(3) << "Nay, made the potential worse: " << Potential << " vs. " << OldPotential << "!" << endl;
-              //Log() << Verbose(3) << "Setting " << *(*runner) << "'s source to " << *Params.DistanceIterators[(*runner)->getNr()]->second << "." << endl;
-              // Undo for Runner (note, we haven't moved the iteration yet, we may use this)
-              Params.PermutationMap[(*runner)->getNr()] = Params.DistanceIterators[(*runner)->getNr()]->second;
-              // Undo for Walker
-              Params.DistanceIterators[(*iter)->getNr()] = Strider;  // take next farther distance target
-              //Log() << Verbose(3) << "Setting " << *(*iter) << "'s source to " << *Params.DistanceIterators[(*iter)->getNr()]->second << "." << endl;
-              Params.PermutationMap[(*iter)->getNr()] = Params.DistanceIterators[(*iter)->getNr()]->second;
-            } else {
-              Params.DistanceIterators[(*runner)->getNr()] = Rider;  // if successful also move the pointer in the iterator list
-              DoLog(3) && (Log() << Verbose(3) << "Found a better permutation, new potential is " << Potential << " vs." << OldPotential << "." << endl);
-              OldPotential = Potential;
-            }
-            if (Potential > Params.PenaltyConstants[2]) {
-              DoeLog(1) && (eLog()<< Verbose(1) << "The two-step permutation procedure did not maintain injectivity!" << endl);
-              exit(255);
-            }
-            //Log() << Verbose(0) << endl;
-          } else {
-            DoeLog(1) && (eLog()<< Verbose(1) << **runner << " was not the owner of " << *Sprinter << "!" << endl);
-            exit(255);
-          }
-        } else {
-          Params.PermutationMap[(*iter)->getNr()] = Params.DistanceIterators[(*iter)->getNr()]->second; // new target has no source!
-        }
-        Params.StepList[(*iter)->getNr()]++; // take next farther distance target
-      }
-    } while (++iter != end());
-  } while ((OlderPotential - OldPotential) > 1e-3);
-  DoLog(1) && (Log() << Verbose(1) << "done." << endl);
-
-
-  /// free memory and return with evaluated potential
-  for (int i=getAtomCount(); i--;)
-    Params.DistanceList[i]->clear();
-  delete[](Params.DistanceList);
-  delete[](Params.DistanceIterators);
-  return ConstrainedPotential(Params);
-};
-
-
-/** Evaluates the (distance-related part) of the constrained potential for the constrained forces.
- * \param *out output stream for debugging
- * \param startstep current MD step giving initial position between which and \a endstep we perform the constrained MD (as further steps are always concatenated)
- * \param endstep step giving final position in constrained MD
- * \param **PermutationMap mapping between the atom label of the initial and the final configuration
- * \param *Force ForceMatrix containing force vectors from the external energy functional minimisation.
- * \todo the constant for the constrained potential distance part is hard-coded independently of the hard-coded value in MinimiseConstrainedPotential()
- */
-void molecule::EvaluateConstrainedForces(int startstep, int endstep, atom **PermutationMap, ForceMatrix *Force)
-{
-  double constant = 10.;
-
-  /// evaluate forces (only the distance to target dependent part) with the final PermutationMap
-  DoLog(1) && (Log() << Verbose(1) << "Calculating forces and adding onto ForceMatrix ... " << endl);
-  BOOST_FOREACH(atom *_atom, atoms) {
-    atom *Sprinter = PermutationMap[_atom->getNr()];
-    // set forces
-    for (int i=NDIM;i++;)
-      Force->Matrix[0][_atom->getNr()][5+i] += 2.*constant*sqrt(_atom->getPositionAtStep(startstep).distance(Sprinter->getPositionAtStep(endstep)));
-  }
-  DoLog(1) && (Log() << Verbose(1) << "done." << endl);
-};
 
 /** Performs a linear interpolation between two desired atomic configurations with a given number of steps.
@@ -514,7 +59,8 @@
   atom **PermutationMap = NULL;
   atom *Sprinter = NULL;
-  if (!MapByIdentity)
-    MinimiseConstrainedPotential(PermutationMap, startstep, endstep, configuration.GetIsAngstroem());
-  else {
+  if (!MapByIdentity) {
+    MinimiseConstrainedPotential Minimiser(atoms);
+    Minimiser(PermutationMap, startstep, endstep, configuration.GetIsAngstroem());
+  } else {
     // TODO: construction of enumeration goes here
     PermutationMap = new atom *[getAtomCount()];
@@ -610,6 +156,8 @@
     // calculate forces and potential
     atom **PermutationMap = NULL;
-    ConstrainedPotentialEnergy = MinimiseConstrainedPotential(PermutationMap,configuration.DoConstrainedMD, 0, configuration.GetIsAngstroem());
-    EvaluateConstrainedForces(configuration.DoConstrainedMD, 0, PermutationMap, &Force);
+    MinimiseConstrainedPotential Minimiser(atoms);
+    //double ConstrainedPotentialEnergy =
+    Minimiser(PermutationMap, configuration.DoConstrainedMD, 0, configuration.GetIsAngstroem());
+    Minimiser.EvaluateConstrainedForces(&Force);
     delete[](PermutationMap);
   }
