source: src/Fragmentation/Exporters/SphericalPointDistribution.cpp@ e139180

/*
 * Project: MoleCuilder
 * Description: creates and alters molecular systems
 * Copyright (C)  2014 Frederik Heber. All rights reserved.
 *
 *
 *   This file is part of MoleCuilder.
 *
 *    MoleCuilder is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 2 of the License, or
 *    (at your option) any later version.
 *
 *    MoleCuilder is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with MoleCuilder.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 * SphericalPointDistribution.cpp
 *
 *  Created on: May 30, 2014
 *      Author: heber
 */

// include config.h
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include "CodePatterns/MemDebug.hpp"

#include "SphericalPointDistribution.hpp"

#include "CodePatterns/Assert.hpp"
#include "CodePatterns/IteratorAdaptors.hpp"
#include "CodePatterns/Log.hpp"
#include "CodePatterns/toString.hpp"

#include <algorithm>
#include <cmath>
#include <functional>
#include <iterator>
#include <limits>
#include <list>
#include <vector>
#include <map>

#include "LinearAlgebra/Line.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;
typedef std::vector<double> DistanceArray_t;

DistanceArray_t calculatePairwiseDistances(
    const std::vector<Vector> &_points,
    const IndexList_t &_indices
    )
{
  DistanceArray_t result;
  for (IndexList_t::const_iterator firstiter = _indices.begin();
      firstiter != _indices.end(); ++firstiter) {
    for (IndexList_t::const_iterator seconditer = firstiter;
        seconditer != _indices.end(); ++seconditer) {
      if (firstiter == seconditer)
        continue;
      const double distance = (_points[*firstiter] - _points[*seconditer]).NormSquared();
      result.push_back(distance);
    }
  }
  return result;
}

// class generator: taken from www.cplusplus.com example std::generate
struct c_unique {
  int current;
  c_unique() {current=0;}
  int operator()() {return current++;}
} UniqueNumber;

/** Returns squared L2 error of the given \a _Matching.
 *
 * We compare the pair-wise distances of each associated matching
 * and check whether these distances each match between \a _old and
 * \a _new.
 *
 * \param _old first set of points (fewer or equal to \a _new)
 * \param _new second set of points
 * \param _Matching matching between the two sets
 * \return pair with L1 and squared L2 error
 */
std::pair<double, double> calculateErrorOfMatching(
    const std::vector<Vector> &_old,
    const std::vector<Vector> &_new,
    const IndexList_t &_Matching)
{
  std::pair<double, double> errors( std::make_pair( 0., 0. ) );

  if (_Matching.size() > 1) {
    LOG(3, "INFO: Matching is " << _Matching);

    // calculate all pair-wise distances
    IndexList_t keys(_Matching.size());
    std::generate (keys.begin(), keys.end(), UniqueNumber);
    const DistanceArray_t firstdistances = calculatePairwiseDistances(_old, keys);
    const DistanceArray_t seconddistances = calculatePairwiseDistances(_new, _Matching);

    ASSERT( firstdistances.size() == seconddistances.size(),
        "calculateL2ErrorOfMatching() - mismatch in pair-wise distance array sizes.");
    DistanceArray_t::const_iterator firstiter = firstdistances.begin();
    DistanceArray_t::const_iterator seconditer = seconddistances.begin();
    for (;(firstiter != firstdistances.end()) && (seconditer != seconddistances.end());
        ++firstiter, ++seconditer) {
      const double gap = *firstiter - *seconditer;
      // L1 error
      if (errors.first < gap)
        errors.first = gap;
      // L2 error
      errors.second += gap*gap;
    }
  } else
    ELOG(3, "calculateErrorOfMatching() - Given matching's size is less than 2.");
  LOG(3, "INFO: Resulting errors for matching (L1, L2): "
      << errors.first << "," << errors.second << ".");

  return errors;
}

SphericalPointDistribution::Polygon_t removeMatchingPoints(
    const SphericalPointDistribution::Polygon_t &_points,
    const IndexList_t &_matchingindices
    )
{
  SphericalPointDistribution::Polygon_t remainingpoints;
  IndexArray_t indices(_matchingindices.begin(), _matchingindices.end());
  std::sort(indices.begin(), indices.end());
  LOG(4, "DEBUG: sorted matching is " << indices);
  IndexArray_t::const_iterator valueiter = indices.begin();
  SphericalPointDistribution::Polygon_t::const_iterator pointiter =
      _points.begin();
  for (unsigned int i=0; i< _points.size(); ++i, ++pointiter) {
    // skip all those in values
    if (*valueiter == i)
      ++valueiter;
    else
      remainingpoints.push_back(*pointiter);
  }
  LOG(4, "DEBUG: remaining indices are " << remainingpoints);

  return remainingpoints;
}

struct MatchingControlStructure {
  bool foundflag;
  double bestL2;
  IndexList_t bestmatching;
  VectorArray_t oldpoints;
  VectorArray_t newpoints;
};

/** Recursive function to go through all possible matchings.
 *
 * \param _MCS structure holding global information to the recursion
 * \param _matching current matching being build up
 * \param _indices contains still available indices
 * \param _matchingsize
 */
void recurseMatchings(
    MatchingControlStructure &_MCS,
    IndexList_t &_matching,
    IndexList_t _indices,
    unsigned int _matchingsize)
{
  LOG(4, "DEBUG: Recursing with current matching " << _matching
      << ", remaining indices " << _indices
      << ", and sought size " << _matching.size()+_matchingsize);
  //!> threshold for L1 error below which matching is immediately acceptable
  const double L1THRESHOLD = 1e-2;
  if (!_MCS.foundflag) {
    LOG(4, "DEBUG: Current matching has size " << _matching.size() << ", places left " << _matchingsize);
    if (_matchingsize > 0) {
      // go through all indices
      for (IndexList_t::iterator iter = _indices.begin();
          (iter != _indices.end()) && (!_MCS.foundflag);) {
        // add index to matching
        _matching.push_back(*iter);
        LOG(5, "DEBUG: Adding " << *iter << " to matching.");
        // remove index but keep iterator to position (is the next to erase element)
        IndexList_t::iterator backupiter = _indices.erase(iter);
        // recurse with decreased _matchingsize
        recurseMatchings(_MCS, _matching, _indices, _matchingsize-1);
        // re-add chosen index and reset index to new position
        _indices.insert(backupiter, _matching.back());
        iter = backupiter;
        // remove index from _matching to make space for the next one
        _matching.pop_back();
      }
      // gone through all indices then exit recursion
      if (_matching.empty())
        _MCS.foundflag = true;
    } else {
      LOG(3, "INFO: Found matching " << _matching);
      // calculate errors
      std::pair<double, double> errors = calculateErrorOfMatching(
          _MCS.oldpoints, _MCS.newpoints, _matching);
      if (errors.first < L1THRESHOLD) {
        _MCS.bestmatching = _matching;
        _MCS.foundflag = true;
      } else if (_MCS.bestL2 > errors.second) {
        _MCS.bestmatching = _matching;
        _MCS.bestL2 = errors.second;
      }
    }
  }
}

/** Convert cartesian to polar coordinates.
 *
 * \param _cartesian vector in cartesian coordinates
 * \return vector containing \f$ (r,\theta, \varphi \f$ tuple for polar coordinates
 */
std::vector<double> getPolarCoordinates(const Vector &_cartesian)
{
  std::vector<double> polar(3,0.);
  const double xsqr = _cartesian[0] * _cartesian[0];
  const double ysqr = _cartesian[1] * _cartesian[1];
  polar[0] = sqrt(xsqr + ysqr + _cartesian[2]*_cartesian[2]);
  if (fabs(_cartesian[2]) < std::numeric_limits<double>::epsilon()*1e4) {
    if (fabs(xsqr + ysqr) < std::numeric_limits<double>::epsilon()*1e4) {
      polar[1] = 0.;
    } else {
      // xsqr + ysqr is always non-negative
      polar[1] = M_PI/2.;
    }
  } else {
    polar[1] = atan( sqrt(xsqr + ysqr)/_cartesian[2]);
    if (_cartesian[2] <= -std::numeric_limits<double>::epsilon()*1e4)
      polar[1] += M_PI;
  }

  if (fabs(_cartesian[0]) < std::numeric_limits<double>::epsilon()*1e4) {
    if (fabs(_cartesian[1]) < std::numeric_limits<double>::epsilon()*1e4) {
      polar[2] = 0.;
    } else if (_cartesian[1] > std::numeric_limits<double>::epsilon()*1e4) {
      polar[2] = M_PI/2.;
    } else {
      polar[2] = -M_PI/2.;
    }
  } else {
    polar[2] = atan ( _cartesian[1]/_cartesian[0] );
    if (_cartesian[0] <= -std::numeric_limits<double>::epsilon()*1e4)
      polar[2] += M_PI;
  }
  return polar;
}

/** Calculate cartesian coordinates from given polar ones.
 *
 * \param _polar vector with polar coordinates
 * \return cartesian coordinates
 */
Vector getCartesianCoordinates(const std::vector<double> &_polar)
{
  Vector cartesian;
  ASSERT( _polar.size() == 3,
      "convertToCartesianCoordinates() - tuples has insufficient components.");
  cartesian[0] = _polar[0] * sin(_polar[1]) * cos(_polar[2]);
  cartesian[1] = _polar[0] * sin(_polar[1]) * sin(_polar[2]);
  cartesian[2] = _polar[0] * cos(_polar[1]);
  return cartesian;
}

/** Rotates a given polygon around x, y, and z axis by the given angles.
 *
 * \param _polygon polygon whose points to rotate
 * \param _angles vector with rotation angles for x,y,z axis
 */
SphericalPointDistribution::Polygon_t rotatePolygon(
    const SphericalPointDistribution::Polygon_t &_polygon,
    const std::vector<double> &_angles)
{
  SphericalPointDistribution::Polygon_t rotated_polygon = _polygon;
  RealSpaceMatrix rotation;
  ASSERT( _angles.size() == 3,
      "rotatePolygon() - not exactly "+toString(3)+" components given.");

  // apply rotation angles
  for (SphericalPointDistribution::Polygon_t::iterator iter = rotated_polygon.begin();
      iter != rotated_polygon.end(); ++iter) {
    // transform to polar
    std::vector<double> polar = getPolarCoordinates(*iter);
    LOG(5, "DEBUG: Converting " << *iter << " to " << polar);
    // sum up difference
    std::transform(
        _angles.begin(), _angles.end(),
        polar.begin(),
        polar.begin(),
        std::plus<double>());
    // convert back
    *iter = getCartesianCoordinates(polar);
    LOG(5, "DEBUG: Converting modified " << polar << " to " << *iter);
  }

  return rotated_polygon;
}


SphericalPointDistribution::Polygon_t
SphericalPointDistribution::matchSphericalPointDistributions(
    const SphericalPointDistribution::Polygon_t &_polygon,
    const SphericalPointDistribution::Polygon_t &_newpolygon
    )
{
  SphericalPointDistribution::Polygon_t remainingpoints;
  VectorArray_t remainingold(_polygon.begin(), _polygon.end());
  VectorArray_t remainingnew(_newpolygon.begin(), _newpolygon.end());
  LOG(2, "INFO: Matching old polygon " << _polygon
      << " with new polygon " << _newpolygon);

  if (_polygon.size() > 0) {
    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);
    }
    LOG(2, "INFO: Best matching is " << MCS.bestmatching);

    // determine rotation angles to align the two point distributions with
    // respect to bestmatching
    std::vector<double> angles(NDIM);
    Vector newCenter;
    Vector oldCenter;
    {
      // calculate center of triangle/line/point consisting of first points of matching
      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);

      // transform to polar coordinates and note difference in angular parts
      std::vector<double> oldpolar = getPolarCoordinates(oldCenter);
      std::vector<double> newpolar = getPolarCoordinates(newCenter);
      std::vector<double> differencepolar;
      std::transform(
          oldpolar.begin(), oldpolar.end(),
          newpolar.begin(),
          std::back_inserter(differencepolar),
          std::minus<double>());
      LOG(3, "INFO: (r,theta,phi) angles are" << differencepolar);
    }
    // rotate _newpolygon
    SphericalPointDistribution::Polygon_t rotated_newpolygon =
        rotatePolygon(_newpolygon, angles);
    LOG(5, "DEBUG: Rotated new polygon is " << rotated_newpolygon);

    const Line RotationAxis(zeroVec, oldCenter);
    const double RotationAngle =
        oldCenter.Angle(remainingold[0])
        - oldCenter.Angle(remainingnew[*MCS.bestmatching.begin()]);
    LOG(5, "DEBUG: Rotate around self is " << RotationAngle
        << " around axis " << RotationAxis);

    for (SphericalPointDistribution::Polygon_t::iterator iter = rotated_newpolygon.begin();
        iter != rotated_newpolygon.end(); ++iter) {
      RotationAxis.rotateVector(*iter, RotationAngle);
    }

    // remove all points in matching and return remaining ones
    SphericalPointDistribution::Polygon_t remainingpoints =
        removeMatchingPoints(rotated_newpolygon, MCS.bestmatching);
    LOG(2, "INFO: Remaining points are " << remainingpoints);
    return remainingpoints;
  } else
    return _newpolygon;
}