Index: src/Fragmentation/Exporters/SphericalPointDistribution.cpp
===================================================================
--- src/Fragmentation/Exporters/SphericalPointDistribution.cpp	(revision 2268600b99b47ddc105767e57e705a445948ef84)
+++ src/Fragmentation/Exporters/SphericalPointDistribution.cpp	(revision bcf13bd2fa7f7304c23208ccae14386a689c2c27)
@@ -53,28 +53,28 @@
 
 #include "LinearAlgebra/Line.hpp"
+#include "LinearAlgebra/Plane.hpp"
 #include "LinearAlgebra/RealSpaceMatrix.hpp"
 #include "LinearAlgebra/Vector.hpp"
 
-typedef std::list<unsigned int> IndexList_t;
-typedef std::vector<unsigned int> IndexArray_t;
-typedef std::vector<Vector> VectorArray_t;
+// static entities
+const double SphericalPointDistribution::SQRT_3(sqrt(3.0));
+const double SphericalPointDistribution::warn_amplitude = 1e-2;
+
 typedef std::vector<double> DistanceArray_t;
 
-// static instances
-const double SphericalPointDistribution::SQRT_3(sqrt(3.0));
-
+inline
 DistanceArray_t calculatePairwiseDistances(
-    const std::vector<Vector> &_returnpolygon,
-    const IndexList_t &_indices
+    const std::vector<Vector> &_points,
+    const SphericalPointDistribution::IndexList_t &_indices
     )
 {
   DistanceArray_t result;
-  for (IndexList_t::const_iterator firstiter = _indices.begin();
+  for (SphericalPointDistribution::IndexList_t::const_iterator firstiter = _indices.begin();
       firstiter != _indices.end(); ++firstiter) {
-    for (IndexList_t::const_iterator seconditer = firstiter;
+    for (SphericalPointDistribution::IndexList_t::const_iterator seconditer = firstiter;
         seconditer != _indices.end(); ++seconditer) {
       if (firstiter == seconditer)
         continue;
-      const double distance = (_returnpolygon[*firstiter] - _returnpolygon[*seconditer]).NormSquared();
+      const double distance = (_points[*firstiter] - _points[*seconditer]).NormSquared();
       result.push_back(distance);
     }
@@ -101,5 +101,5 @@
  * \return pair with L1 and squared L2 error
  */
-std::pair<double, double> calculateErrorOfMatching(
+std::pair<double, double> SphericalPointDistribution::calculateErrorOfMatching(
     const std::vector<Vector> &_old,
     const std::vector<Vector> &_new,
@@ -138,5 +138,5 @@
 }
 
-SphericalPointDistribution::Polygon_t removeMatchingPoints(
+SphericalPointDistribution::Polygon_t SphericalPointDistribution::removeMatchingPoints(
     const VectorArray_t &_points,
     const IndexList_t &_matchingindices
@@ -163,12 +163,4 @@
 }
 
-struct MatchingControlStructure {
-  bool foundflag;
-  double bestL2;
-  IndexList_t bestmatching;
-  VectorArray_t oldreturnpolygon;
-  VectorArray_t newreturnpolygon;
-};
-
 /** Recursive function to go through all possible matchings.
  *
@@ -178,5 +170,5 @@
  * \param _matchingsize
  */
-void recurseMatchings(
+void SphericalPointDistribution::recurseMatchings(
     MatchingControlStructure &_MCS,
     IndexList_t &_matching,
@@ -215,5 +207,5 @@
       // calculate errors
       std::pair<double, double> errors = calculateErrorOfMatching(
-          _MCS.oldreturnpolygon, _MCS.newreturnpolygon, _matching);
+          _MCS.oldpoints, _MCS.newpoints, _matching);
       if (errors.first < L1THRESHOLD) {
         _MCS.bestmatching = _matching;
@@ -227,31 +219,270 @@
 }
 
-/** Rotates a given polygon around x, y, and z axis by the given angles.
- *
- * \param _polygon polygon whose points to rotate
- * \param _q quaternion specifying the rotation of the coordinate system
+/** Decides by an orthonormal third vector whether the sign of the rotation
+ * angle should be negative or positive.
+ *
+ * \return -1 or 1
  */
-SphericalPointDistribution::Polygon_t rotatePolygon(
+inline
+double determineSignOfRotation(
+    const Vector &_oldPosition,
+    const Vector &_newPosition,
+    const Vector &_RotationAxis
+    )
+{
+  Vector dreiBein(_oldPosition);
+  dreiBein.VectorProduct(_RotationAxis);
+  dreiBein.Normalize();
+  const double sign =
+      (dreiBein.ScalarProduct(_newPosition) < 0.) ? -1. : +1.;
+  LOG(6, "DEBUG: oldCenter on plane is " << _oldPosition
+      << ", newCenter in plane is " << _newPosition
+      << ", and dreiBein is " << dreiBein);
+  return sign;
+}
+
+/** Finds combinatorially the best matching between points in \a _polygon
+ * and \a _newpolygon.
+ *
+ * We find the matching with the smallest L2 error, where we break when we stumble
+ * upon a matching with zero error.
+ *
+ * \sa recurseMatchings() for going through all matchings
+ *
+ * \param _polygon here, we have indices 0,1,2,...
+ * \param _newpolygon and here we need to find the correct indices
+ * \return list of indices: first in \a _polygon goes to first index for \a _newpolygon
+ */
+SphericalPointDistribution::IndexList_t SphericalPointDistribution::findBestMatching(
     const SphericalPointDistribution::Polygon_t &_polygon,
-    const boost::math::quaternion<double> &_q)
-{
-  SphericalPointDistribution::Polygon_t rotated_polygon = _polygon;
-  boost::math::quaternion<double> q_inverse =
-      boost::math::conj(_q)/(boost::math::norm(_q));
-
-  // apply rotation angles
-  for (SphericalPointDistribution::Polygon_t::iterator iter = rotated_polygon.begin();
-      iter != rotated_polygon.end(); ++iter) {
-    Vector &current = *iter;
-    boost::math::quaternion<double> p(0, current[0], current[1], current[2]);
-    p = _q * p * q_inverse;
-    LOG(5, "DEBUG: Rotated point is " << p);
-    // i have no idea why but first component comes up with wrong sign
-    current[0] = -p.R_component_2();
-    current[1] = p.R_component_3();
-    current[2] = p.R_component_4();
-  }
-
-  return rotated_polygon;
+    const SphericalPointDistribution::Polygon_t &_newpolygon
+    )
+{
+  MatchingControlStructure MCS;
+  MCS.foundflag = false;
+  MCS.bestL2 = std::numeric_limits<double>::max();
+  MCS.oldpoints.insert(MCS.oldpoints.begin(), _polygon.begin(),_polygon.end() );
+  MCS.newpoints.insert(MCS.newpoints.begin(), _newpolygon.begin(),_newpolygon.end() );
+
+  // search for bestmatching combinatorially
+  {
+    // translate polygon into vector to enable index addressing
+    IndexList_t indices(_newpolygon.size());
+    std::generate(indices.begin(), indices.end(), UniqueNumber);
+    IndexList_t matching;
+
+    // walk through all matchings
+    const unsigned int matchingsize = _polygon.size();
+    ASSERT( matchingsize <= indices.size(),
+        "SphericalPointDistribution::matchSphericalPointDistributions() - not enough new points to choose for matching to old ones.");
+    recurseMatchings(MCS, matching, indices, matchingsize);
+  }
+  return MCS.bestmatching;
+}
+
+inline
+Vector calculateCenter(
+    const SphericalPointDistribution::VectorArray_t &_positions,
+    const SphericalPointDistribution::IndexList_t &_indices)
+{
+  Vector Center;
+  Center.Zero();
+  for (SphericalPointDistribution::IndexList_t::const_iterator iter = _indices.begin();
+      iter != _indices.end(); ++iter)
+    Center += _positions[*iter];
+  if (!_indices.empty())
+    Center *= 1./(double)_indices.size();
+
+  return Center;
+}
+
+inline
+void calculateOldAndNewCenters(
+    Vector &_oldCenter,
+    Vector &_newCenter,
+    const SphericalPointDistribution::VectorArray_t &_referencepositions,
+    const SphericalPointDistribution::VectorArray_t &_currentpositions,
+    const SphericalPointDistribution::IndexList_t &_bestmatching)
+{
+  const size_t NumberIds = std::min(_bestmatching.size(), (size_t)3);
+  SphericalPointDistribution::IndexList_t continuousIds(NumberIds, -1);
+  std::generate(continuousIds.begin(), continuousIds.end(), UniqueNumber);
+  _oldCenter = calculateCenter(_referencepositions, continuousIds);
+  // C++11 defines a copy_n function ...
+  SphericalPointDistribution::IndexList_t::const_iterator enditer = _bestmatching.begin();
+  std::advance(enditer, NumberIds);
+  SphericalPointDistribution::IndexList_t firstbestmatchingIds(NumberIds, -1);
+  std::copy(_bestmatching.begin(), enditer, firstbestmatchingIds.begin());
+  _newCenter = calculateCenter( _currentpositions, firstbestmatchingIds);
+}
+
+SphericalPointDistribution::Rotation_t SphericalPointDistribution::findPlaneAligningRotation(
+    const VectorArray_t &_referencepositions,
+    const VectorArray_t &_currentpositions,
+    const IndexList_t &_bestmatching
+    )
+{
+  bool dontcheck = false;
+  // initialize to no rotation
+  Rotation_t Rotation;
+  Rotation.first.Zero();
+  Rotation.first[0] = 1.;
+  Rotation.second = 0.;
+
+  // calculate center of triangle/line/point consisting of first points of matching
+  Vector oldCenter;
+  Vector newCenter;
+  calculateOldAndNewCenters(
+      oldCenter, newCenter,
+      _referencepositions, _currentpositions, _bestmatching);
+
+  if ((!oldCenter.IsZero()) && (!newCenter.IsZero())) {
+    LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
+    oldCenter.Normalize();
+    newCenter.Normalize();
+    if (!oldCenter.IsEqualTo(newCenter)) {
+      // calculate rotation axis and angle
+      Rotation.first = oldCenter;
+      Rotation.first.VectorProduct(newCenter);
+      Rotation.second = oldCenter.Angle(newCenter); // /(M_PI/2.);
+    } else {
+      // no rotation required anymore
+    }
+  } else {
+    LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
+    if ((oldCenter.IsZero()) && (newCenter.IsZero())) {
+      // either oldCenter or newCenter (or both) is directly at origin
+      if (_bestmatching.size() == 2) {
+        // line case
+        Vector oldPosition = _currentpositions[*_bestmatching.begin()];
+        Vector newPosition = _referencepositions[0];
+        // check whether we need to rotate at all
+        if (!oldPosition.IsEqualTo(newPosition)) {
+          Rotation.first = oldPosition;
+          Rotation.first.VectorProduct(newPosition);
+          // orientation will fix the sign here eventually
+          Rotation.second = oldPosition.Angle(newPosition);
+        } else {
+          // no rotation required anymore
+        }
+      } else {
+        // triangle case
+        // both triangles/planes have same center, hence get axis by
+        // VectorProduct of Normals
+        Plane newplane(_referencepositions[0], _referencepositions[1], _referencepositions[2]);
+        VectorArray_t vectors;
+        for (IndexList_t::const_iterator iter = _bestmatching.begin();
+            iter != _bestmatching.end(); ++iter)
+          vectors.push_back(_currentpositions[*iter]);
+        Plane oldplane(vectors[0], vectors[1], vectors[2]);
+        Vector oldPosition = oldplane.getNormal();
+        Vector newPosition = newplane.getNormal();
+        // check whether we need to rotate at all
+        if (!oldPosition.IsEqualTo(newPosition)) {
+          Rotation.first = oldPosition;
+          Rotation.first.VectorProduct(newPosition);
+          Rotation.first.Normalize();
+
+          // construct reference vector to determine direction of rotation
+          const double sign = determineSignOfRotation(oldPosition, newPosition, Rotation.first);
+          Rotation.second = sign * oldPosition.Angle(newPosition);
+          LOG(5, "DEBUG: Rotating plane normals by " << Rotation.second
+              << " around axis " << Rotation.first);
+        } else {
+          // else do nothing
+        }
+      }
+    } else {
+      // TODO: we can't do anything here, but this case needs to be dealt with when
+      // we have no ideal geometries anymore
+      if ((oldCenter-newCenter).Norm() > warn_amplitude)
+        ELOG(2, "oldCenter is " << oldCenter << ", yet newCenter is " << newCenter);
+      // else they are considered close enough
+      dontcheck = true;
+    }
+  }
+
+#ifndef NDEBUG
+  // check: rotation brings newCenter onto oldCenter position
+  if (!dontcheck) {
+    Line Axis(zeroVec, Rotation.first);
+    Vector test = Axis.rotateVector(newCenter, Rotation.second);
+    LOG(4, "CHECK: rotated newCenter is " << test
+        << ", oldCenter is " << oldCenter);
+    ASSERT( (test - oldCenter).NormSquared() < std::numeric_limits<double>::epsilon()*1e4,
+        "matchSphericalPointDistributions() - rotation does not work as expected by "
+        +toString((test - oldCenter).NormSquared())+".");
+  }
+#endif
+
+  return Rotation;
+}
+
+SphericalPointDistribution::Rotation_t SphericalPointDistribution::findPointAligningRotation(
+    const VectorArray_t &remainingold,
+    const VectorArray_t &remainingnew,
+    const IndexList_t &_bestmatching)
+{
+  // initialize rotation to zero
+  Rotation_t Rotation;
+  Rotation.first.Zero();
+  Rotation.first[0] = 1.;
+  Rotation.second = 0.;
+
+  // recalculate center
+  Vector oldCenter;
+  Vector newCenter;
+  calculateOldAndNewCenters(
+      oldCenter, newCenter,
+      remainingold, remainingnew, _bestmatching);
+
+  Vector oldPosition = remainingnew[*_bestmatching.begin()];
+  Vector newPosition = remainingold[0];
+  LOG(6, "DEBUG: oldPosition is " << oldPosition << " and newPosition is " << newPosition);
+  if (!oldPosition.IsEqualTo(newPosition)) {
+    if ((!oldCenter.IsZero()) && (!newCenter.IsZero())) {
+      oldCenter.Normalize();  // note weighted sum of normalized weight is not normalized
+      Rotation.first = oldCenter;
+      LOG(6, "DEBUG: Picking normalized oldCenter as Rotation.first " << oldCenter);
+      oldPosition.ProjectOntoPlane(Rotation.first);
+      newPosition.ProjectOntoPlane(Rotation.first);
+      LOG(6, "DEBUG: Positions after projection are " << oldPosition << " and " << newPosition);
+    } else {
+      if (_bestmatching.size() == 2) {
+        // line situation
+        try {
+          Plane oldplane(oldPosition, oldCenter, newPosition);
+          Rotation.first = oldplane.getNormal();
+          LOG(6, "DEBUG: Plane is " << oldplane << " and normal is " << Rotation.first);
+        } catch (LinearDependenceException &e) {
+          LOG(6, "DEBUG: Vectors defining plane are linearly dependent.");
+          // oldPosition and newPosition are on a line, just flip when not equal
+          if (!oldPosition.IsEqualTo(newPosition)) {
+            Rotation.first.Zero();
+            Rotation.first.GetOneNormalVector(oldPosition);
+            LOG(6, "DEBUG: For flipping we use Rotation.first " << Rotation.first);
+            assert( Rotation.first.ScalarProduct(oldPosition) < std::numeric_limits<double>::epsilon()*1e4);
+  //              Rotation.second = M_PI;
+          } else {
+            LOG(6, "DEBUG: oldPosition and newPosition are equivalent.");
+          }
+        }
+      } else {
+        // triangle situation
+        Plane oldplane(remainingold[0], remainingold[1], remainingold[2]);
+        Rotation.first = oldplane.getNormal();
+        LOG(6, "DEBUG: oldPlane is " << oldplane << " and normal is " << Rotation.first);
+        oldPosition.ProjectOntoPlane(Rotation.first);
+        LOG(6, "DEBUG: Positions after projection are " << oldPosition << " and " << newPosition);
+      }
+    }
+    // construct reference vector to determine direction of rotation
+    const double sign = determineSignOfRotation(oldPosition, newPosition, Rotation.first);
+    Rotation.second = sign * oldPosition.Angle(newPosition);
+  } else {
+    LOG(6, "DEBUG: oldPosition and newPosition are equivalent, hence no orientating rotation.");
+  }
+
+  return Rotation;
 }
 
@@ -269,109 +500,96 @@
       << " with new polygon " << _newpolygon);
 
+  if (_polygon.size() == _newpolygon.size()) {
+    // same number of points desired as are present? Do nothing
+    LOG(2, "INFO: There are no vacant points to return.");
+    return remainingreturnpolygon;
+  }
+
   if (_polygon.size() > 0) {
-    MatchingControlStructure MCS;
-    MCS.foundflag = false;
-    MCS.bestL2 = std::numeric_limits<double>::max();
-    MCS.oldreturnpolygon.insert(MCS.oldreturnpolygon.begin(), _polygon.begin(),_polygon.end() );
-    MCS.newreturnpolygon.insert(MCS.newreturnpolygon.begin(), _newpolygon.begin(),_newpolygon.end() );
-
-    // search for bestmatching combinatorially
+    IndexList_t bestmatching = findBestMatching(_polygon, _newpolygon);
+    LOG(2, "INFO: Best matching is " << bestmatching);
+
+    // determine rotation angles to align the two point distributions with
+    // respect to bestmatching:
+    // we use the center between the three first matching points
+    /// the first rotation brings these two centers to coincide
+    VectorArray_t rotated_newpolygon = remainingnew;
     {
-      // translate polygon into vector to enable index addressing
-      IndexList_t indices(_newpolygon.size());
-      std::generate(indices.begin(), indices.end(), UniqueNumber);
-      IndexList_t matching;
-
-      // walk through all matchings
-      const unsigned int matchingsize = _polygon.size();
-      ASSERT( matchingsize <= indices.size(),
-          "SphericalPointDistribution::matchSphericalPointDistributions() - not enough new returnpolygon to choose for matching to old ones.");
-      recurseMatchings(MCS, matching, indices, matchingsize);
-    }
-    LOG(2, "INFO: Best matching is " << MCS.bestmatching);
-
-    // determine rotation angles to align the two point distributions with
-    // respect to bestmatching
-    VectorArray_t rotated_newpolygon = remainingnew;
-    Vector oldCenter;
-    {
-      // calculate center of triangle/line/point consisting of first points of matching
-      Vector newCenter;
-      IndexList_t::const_iterator iter = MCS.bestmatching.begin();
-      unsigned int i = 0;
-      for (; (i<3) && (i<MCS.bestmatching.size()); ++i, ++iter) {
-        oldCenter += remainingold[i];
-        newCenter += remainingnew[*iter];
-      }
-      oldCenter *= 1./(double)i;
-      newCenter *= 1./(double)i;
-      LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
-
-      if ((oldCenter - newCenter).NormSquared() > std::numeric_limits<double>::epsilon()*1e4) {
-        // setup quaternion
-        Vector RotationAxis = oldCenter;
-        RotationAxis.VectorProduct(newCenter);
-        Line Axis(zeroVec, RotationAxis);
-        RotationAxis.Normalize();
-        const double RotationAngle = oldCenter.Angle(newCenter); // /(M_PI/2.);
-        LOG(5, "DEBUG: Rotate coordinate system by " << RotationAngle
-            << " around axis " << RotationAxis);
-
-        // apply rotation angles
-        for (VectorArray_t::iterator iter = rotated_newpolygon.begin();
-            iter != rotated_newpolygon.end(); ++iter) {
-          Vector &current = *iter;
-          LOG(5, "DEBUG: Original point is " << current);
-          current =  Axis.rotateVector(current, RotationAngle);
-          LOG(5, "DEBUG: Rotated point is " << current);
+      Rotation_t Rotation = findPlaneAligningRotation(
+          remainingold,
+          remainingnew,
+          bestmatching);
+      LOG(5, "DEBUG: Rotating coordinate system by " << Rotation.second
+          << " around axis " << Rotation.first);
+      Line Axis(zeroVec, Rotation.first);
+
+      // apply rotation angle to bring newCenter to oldCenter
+      for (VectorArray_t::iterator iter = rotated_newpolygon.begin();
+          iter != rotated_newpolygon.end(); ++iter) {
+        Vector &current = *iter;
+        LOG(6, "DEBUG: Original point is " << current);
+        current =  Axis.rotateVector(current, Rotation.second);
+        LOG(6, "DEBUG: Rotated point is " << current);
+      }
+
+#ifndef NDEBUG
+      // check: rotated "newCenter" should now equal oldCenter
+      {
+        Vector oldCenter;
+        Vector rotatednewCenter;
+        calculateOldAndNewCenters(
+            oldCenter, rotatednewCenter,
+            remainingold, rotated_newpolygon, bestmatching);
+        // NOTE: Center must not necessarily lie on the sphere with norm 1, hence, we
+        // have to normalize it just as before, as oldCenter and newCenter lengths may differ.
+        if ((!oldCenter.IsZero()) && (!rotatednewCenter.IsZero())) {
+          oldCenter.Normalize();
+          rotatednewCenter.Normalize();
+          LOG(4, "CHECK: rotatednewCenter is " << rotatednewCenter
+              << ", oldCenter is " << oldCenter);
+          ASSERT( (rotatednewCenter - oldCenter).NormSquared() < std::numeric_limits<double>::epsilon()*1e4,
+              "matchSphericalPointDistributions() - rotation does not work as expected by "
+              +toString((rotatednewCenter - oldCenter).NormSquared())+".");
         }
       }
-    }
-    // rotate triangle/line/point around itself to match orientation
-    if (MCS.bestmatching.size() > 1) {
-      if (oldCenter.NormSquared() > std::numeric_limits<double>::epsilon()*1e4) {
-        // construct RotationAxis and two points on its plane, defining the angle
-        const Line RotationAxis(zeroVec, oldCenter);
-        Vector oldPosition(rotated_newpolygon[*MCS.bestmatching.begin()]);
-        oldPosition.ProjectOntoPlane(RotationAxis.getDirection());
-        Vector newPosition(remainingold[*MCS.bestmatching.begin()]);
-        newPosition.ProjectOntoPlane(RotationAxis.getDirection());
-
-        // construct reference vector to determine direction of rotation
-        Vector dreiBein(oldPosition);
-        dreiBein.VectorProduct(oldCenter);
-        dreiBein.Normalize();
-        const double sign =
-            (dreiBein.ScalarProduct(newPosition) < 0.) ? -1. : +1.;
-        LOG(6, "DEBUG: oldCenter on plane is " << oldPosition
-            << ", newCenter in plane is " << newPosition
-            << ", and dreiBein is " << dreiBein);
-        const double RotationAngle = sign * oldPosition.Angle(newPosition);
-        LOG(5, "DEBUG: Rotating around self is " << RotationAngle
-            << " around axis " << RotationAxis);
+#endif
+    }
+    /// the second (orientation) rotation aligns the planes such that the
+    /// points themselves coincide
+    if (bestmatching.size() > 1) {
+      Rotation_t Rotation = findPointAligningRotation(
+          remainingold,
+          rotated_newpolygon,
+          bestmatching);
+
+      // construct RotationAxis and two points on its plane, defining the angle
+      Rotation.first.Normalize();
+      const Line RotationAxis(zeroVec, Rotation.first);
+
+      LOG(5, "DEBUG: Rotating around self is " << Rotation.second
+          << " around axis " << RotationAxis);
 
 #ifndef NDEBUG
-        // check: first bestmatching in rotated_newpolygon and remainingnew
-        // should now equal
-        {
-          const IndexList_t::const_iterator iter = MCS.bestmatching.begin();
-          Vector rotatednew = RotationAxis.rotateVector(
-              rotated_newpolygon[*iter],
-              RotationAngle);
-          LOG(4, "CHECK: rotated first new bestmatching is " << rotatednew
-              << " while old was " << remainingold[*iter]);
-          ASSERT( (rotatednew - remainingold[*iter]).Norm()
-              < std::numeric_limits<double>::epsilon()*1e4,
-              "matchSphericalPointDistributions() - orientation rotation does not work as expected.");
-        }
+      // check: first bestmatching in rotated_newpolygon and remainingnew
+      // should now equal
+      {
+        const IndexList_t::const_iterator iter = bestmatching.begin();
+        Vector rotatednew = RotationAxis.rotateVector(
+            rotated_newpolygon[*iter],
+            Rotation.second);
+        LOG(4, "CHECK: rotated first new bestmatching is " << rotatednew
+            << " while old was " << remainingold[0]);
+        ASSERT( (rotatednew - remainingold[0]).Norm() < warn_amplitude,
+            "matchSphericalPointDistributions() - orientation rotation ends up off by more than "
+            +toString(warn_amplitude)+".");
+      }
 #endif
 
-        for (VectorArray_t::iterator iter = rotated_newpolygon.begin();
-            iter != rotated_newpolygon.end(); ++iter) {
-          Vector &current = *iter;
-          LOG(6, "DEBUG: Original point is " << current);
-          current = RotationAxis.rotateVector(current, RotationAngle);
-          LOG(6, "DEBUG: Rotated point is " << current);
-        }
+      for (VectorArray_t::iterator iter = rotated_newpolygon.begin();
+          iter != rotated_newpolygon.end(); ++iter) {
+        Vector &current = *iter;
+        LOG(6, "DEBUG: Original point is " << current);
+        current = RotationAxis.rotateVector(current, Rotation.second);
+        LOG(6, "DEBUG: Rotated point is " << current);
       }
     }
@@ -379,5 +597,5 @@
     // remove all points in matching and return remaining ones
     SphericalPointDistribution::Polygon_t remainingpoints =
-        removeMatchingPoints(rotated_newpolygon, MCS.bestmatching);
+        removeMatchingPoints(rotated_newpolygon, bestmatching);
     LOG(2, "INFO: Remaining points are " << remainingpoints);
     return remainingpoints;
Index: src/Fragmentation/Exporters/SphericalPointDistribution.hpp
===================================================================
--- src/Fragmentation/Exporters/SphericalPointDistribution.hpp	(revision 2268600b99b47ddc105767e57e705a445948ef84)
+++ src/Fragmentation/Exporters/SphericalPointDistribution.hpp	(revision bcf13bd2fa7f7304c23208ccae14386a689c2c27)
@@ -76,4 +76,55 @@
   //!> precalculated value for root of 3
   static const double SQRT_3;
+
+  typedef std::pair<Vector, double> Rotation_t;
+
+  typedef std::list<unsigned int> IndexList_t;
+  typedef std::vector<unsigned int> IndexArray_t;
+  typedef std::vector<Vector> VectorArray_t;
+
+  //!> amplitude up to which deviations in checks of rotations are tolerated
+  static const double warn_amplitude;
+
+private:
+  static std::pair<double, double> calculateErrorOfMatching(
+      const std::vector<Vector> &_old,
+      const std::vector<Vector> &_new,
+      const IndexList_t &_Matching);
+
+  static Polygon_t removeMatchingPoints(
+      const VectorArray_t &_points,
+      const IndexList_t &_matchingindices
+      );
+
+  struct MatchingControlStructure {
+    bool foundflag;
+    double bestL2;
+    IndexList_t bestmatching;
+    VectorArray_t oldpoints;
+    VectorArray_t newpoints;
+  };
+
+  static void recurseMatchings(
+      MatchingControlStructure &_MCS,
+      IndexList_t &_matching,
+      IndexList_t _indices,
+      unsigned int _matchingsize);
+
+  static IndexList_t findBestMatching(
+      const Polygon_t &_polygon,
+      const Polygon_t &_newpolygon
+      );
+
+  static Rotation_t findPlaneAligningRotation(
+      const VectorArray_t &_referencepositions,
+      const VectorArray_t &_currentpositions,
+      const IndexList_t &_bestmatching
+      );
+
+  static Rotation_t findPointAligningRotation(
+      const VectorArray_t &remainingold,
+      const VectorArray_t &remainingnew,
+      const IndexList_t &_bestmatching);
+
 };
 
Index: src/Fragmentation/Exporters/unittests/Makefile.am
===================================================================
--- src/Fragmentation/Exporters/unittests/Makefile.am	(revision 2268600b99b47ddc105767e57e705a445948ef84)
+++ src/Fragmentation/Exporters/unittests/Makefile.am	(revision bcf13bd2fa7f7304c23208ccae14386a689c2c27)
@@ -20,5 +20,4 @@
 	SphericalPointDistributionUnitTest
 
-XFAIL_TESTS += SphericalPointDistributionUnitTest
 TESTS += $(FRAGMENTATIONEXPORTERSTESTS)
 check_PROGRAMS += $(FRAGMENTATIONEXPORTERSTESTS) 
Index: src/Fragmentation/Exporters/unittests/SphericalPointDistributionUnitTest.cpp
===================================================================
--- src/Fragmentation/Exporters/unittests/SphericalPointDistributionUnitTest.cpp	(revision 2268600b99b47ddc105767e57e705a445948ef84)
+++ src/Fragmentation/Exporters/unittests/SphericalPointDistributionUnitTest.cpp	(revision bcf13bd2fa7f7304c23208ccae14386a689c2c27)
@@ -150,4 +150,100 @@
 }
 
+void perturbPolygon(
+    SphericalPointDistribution::Polygon_t &_polygon,
+    double _amplitude
+    )
+{
+  for (SphericalPointDistribution::Polygon_t::iterator iter = _polygon.begin();
+      iter != _polygon.end(); ++iter) {
+    Vector perturber;
+    perturber.GetOneNormalVector((*iter));
+    perturber.Scale(_amplitude);
+    *iter = *iter + perturber;
+    (*iter).Normalize();
+  }
+}
+
+static
+bool areEqualToWithinBounds(
+    const SphericalPointDistribution::Polygon_t &_polygon,
+    const SphericalPointDistribution::Polygon_t &_otherpolygon,
+    double _amplitude
+    )
+{
+  // same size?
+  if (_polygon.size() != _otherpolygon.size())
+    return false;
+  // same points ? We just check witrh trivial mapping, nothing fancy ...
+  bool status = true;
+  SphericalPointDistribution::Polygon_t::const_iterator iter = _polygon.begin();
+  SphericalPointDistribution::Polygon_t::const_iterator otheriter = _otherpolygon.begin();
+  for (; iter != _polygon.end(); ++iter, ++otheriter) {
+    status &= (*iter - *otheriter).Norm() < _amplitude;
+  }
+  return status;
+}
+
+/** UnitTest for areEqualToWithinBounds()
+ */
+void SphericalPointDistributionTest::areEqualToWithinBoundsTest()
+{
+  // test with no points
+  {
+    SphericalPointDistribution::Polygon_t polygon;
+    SphericalPointDistribution::Polygon_t expected = polygon;
+    CPPUNIT_ASSERT( areEqualToWithinBounds(polygon, expected, std::numeric_limits<double>::epsilon()*1e2) );
+  }
+  // test with one point
+  {
+    SphericalPointDistribution::Polygon_t polygon;
+    polygon += Vector(1.,0.,0.);
+    SphericalPointDistribution::Polygon_t expected = polygon;
+    CPPUNIT_ASSERT( areEqualToWithinBounds(polygon, expected, std::numeric_limits<double>::epsilon()*1e2) );
+  }
+  // test with two points
+  {
+    SphericalPointDistribution::Polygon_t polygon;
+    polygon += Vector(1.,0.,0.);
+    polygon += Vector(0.,1.,0.);
+    SphericalPointDistribution::Polygon_t expected = polygon;
+    CPPUNIT_ASSERT( areEqualToWithinBounds(polygon, expected, std::numeric_limits<double>::epsilon()*1e2) );
+  }
+
+  // test with two points in different order: THIS GOES WRONG: We only check trivially
+  {
+    SphericalPointDistribution::Polygon_t polygon;
+    polygon += Vector(1.,0.,0.);
+    polygon += Vector(0.,1.,0.);
+    SphericalPointDistribution::Polygon_t expected;
+    expected += Vector(0.,1.,0.);
+    expected += Vector(1.,0.,0.);
+    CPPUNIT_ASSERT( !areEqualToWithinBounds(polygon, expected, std::numeric_limits<double>::epsilon()*1e2) );
+  }
+
+  // test with two different points
+  {
+    SphericalPointDistribution::Polygon_t polygon;
+    polygon += Vector(1.,0.,0.);
+    polygon += Vector(0.,1.,0.);
+    SphericalPointDistribution::Polygon_t expected;
+    expected += Vector(1.01,0.,0.);
+    expected += Vector(0.,1.,0.);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(polygon, expected, 0.05) );
+    CPPUNIT_ASSERT( !areEqualToWithinBounds(polygon, expected, 0.005) );
+  }
+
+  // test with different number of points
+  {
+    SphericalPointDistribution::Polygon_t polygon;
+    polygon += Vector(1.,0.,0.);
+    polygon += Vector(0.,1.,0.);
+    SphericalPointDistribution::Polygon_t expected;
+    expected += Vector(0.,1.,0.);
+    CPPUNIT_ASSERT( !areEqualToWithinBounds(polygon, expected, 0.05) );
+  }
+}
+
+
 /** UnitTest for matchSphericalPointDistributions() with three points
  */
@@ -205,4 +301,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -227,4 +327,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -241,4 +345,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -260,4 +368,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 }
@@ -318,4 +430,28 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
+  }
+
+  // test with two points, matching trivially, also with slightly perturbed
+  {
+    SphericalPointDistribution::Polygon_t polygon;
+    polygon += Vector(1.,0.,0.), Vector(-1./3.0, 2.0*M_SQRT2/3.0,0.);
+    SphericalPointDistribution::Polygon_t newpolygon =
+        SPD.get<4>();
+    SphericalPointDistribution::Polygon_t expected = newpolygon;
+    expected.pop_front(); // remove first point
+    expected.pop_front(); // remove second point
+    SphericalPointDistribution::Polygon_t remaining =
+        SphericalPointDistribution::matchSphericalPointDistributions(
+            polygon,
+            newpolygon);
+    CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -340,4 +476,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -360,4 +500,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -384,4 +528,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 }
@@ -442,4 +590,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -494,4 +646,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -518,4 +674,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 }
@@ -576,4 +736,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -628,4 +792,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -652,4 +820,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 }
@@ -710,4 +882,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -762,4 +938,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -786,4 +966,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 }
@@ -844,4 +1028,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -900,4 +1088,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 
@@ -924,4 +1116,8 @@
             newpolygon);
     CPPUNIT_ASSERT_EQUAL( expected, remaining );
+    // also slightly perturbed
+    const double amplitude = 0.05;
+    perturbPolygon(polygon, amplitude);
+    CPPUNIT_ASSERT( areEqualToWithinBounds(expected, remaining, amplitude) );
   }
 }
Index: src/Fragmentation/Exporters/unittests/SphericalPointDistributionUnitTest.hpp
===================================================================
--- src/Fragmentation/Exporters/unittests/SphericalPointDistributionUnitTest.hpp	(revision 2268600b99b47ddc105767e57e705a445948ef84)
+++ src/Fragmentation/Exporters/unittests/SphericalPointDistributionUnitTest.hpp	(revision bcf13bd2fa7f7304c23208ccae14386a689c2c27)
@@ -22,4 +22,5 @@
 {
     CPPUNIT_TEST_SUITE( SphericalPointDistributionTest) ;
+    CPPUNIT_TEST ( areEqualToWithinBoundsTest );
     CPPUNIT_TEST ( matchSphericalPointDistributionsTest_2 );
     CPPUNIT_TEST ( matchSphericalPointDistributionsTest_3 );
@@ -34,4 +35,5 @@
       void setUp();
       void tearDown();
+      void areEqualToWithinBoundsTest();
       void matchSphericalPointDistributionsTest_2();
       void matchSphericalPointDistributionsTest_3();
