| [f54930] | 1 | /*
 | 
|---|
 | 2 |  * Project: MoleCuilder
 | 
|---|
 | 3 |  * Description: creates and alters molecular systems
 | 
|---|
 | 4 |  * Copyright (C)  2014 Frederik Heber. All rights reserved.
 | 
|---|
 | 5 |  *
 | 
|---|
 | 6 |  *
 | 
|---|
 | 7 |  *   This file is part of MoleCuilder.
 | 
|---|
 | 8 |  *
 | 
|---|
 | 9 |  *    MoleCuilder is free software: you can redistribute it and/or modify
 | 
|---|
 | 10 |  *    it under the terms of the GNU General Public License as published by
 | 
|---|
 | 11 |  *    the Free Software Foundation, either version 2 of the License, or
 | 
|---|
 | 12 |  *    (at your option) any later version.
 | 
|---|
 | 13 |  *
 | 
|---|
 | 14 |  *    MoleCuilder is distributed in the hope that it will be useful,
 | 
|---|
 | 15 |  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 | 
|---|
 | 16 |  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 | 
|---|
 | 17 |  *    GNU General Public License for more details.
 | 
|---|
 | 18 |  *
 | 
|---|
 | 19 |  *    You should have received a copy of the GNU General Public License
 | 
|---|
 | 20 |  *    along with MoleCuilder.  If not, see <http://www.gnu.org/licenses/>.
 | 
|---|
 | 21 |  */
 | 
|---|
 | 22 | 
 | 
|---|
 | 23 | /*
 | 
|---|
 | 24 |  * SphericalPointDistribution.cpp
 | 
|---|
 | 25 |  *
 | 
|---|
 | 26 |  *  Created on: May 30, 2014
 | 
|---|
 | 27 |  *      Author: heber
 | 
|---|
 | 28 |  */
 | 
|---|
 | 29 | 
 | 
|---|
 | 30 | // include config.h
 | 
|---|
 | 31 | #ifdef HAVE_CONFIG_H
 | 
|---|
 | 32 | #include <config.h>
 | 
|---|
 | 33 | #endif
 | 
|---|
 | 34 | 
 | 
|---|
 | 35 | #include "CodePatterns/MemDebug.hpp"
 | 
|---|
 | 36 | 
 | 
|---|
 | 37 | #include "SphericalPointDistribution.hpp"
 | 
|---|
 | 38 | 
 | 
|---|
 | 39 | #include "CodePatterns/Assert.hpp"
 | 
|---|
| [d468b5] | 40 | #include "CodePatterns/IteratorAdaptors.hpp"
 | 
|---|
| [81557e] | 41 | #include "CodePatterns/Log.hpp"
 | 
|---|
| [d468b5] | 42 | #include "CodePatterns/toString.hpp"
 | 
|---|
| [f54930] | 43 | 
 | 
|---|
 | 44 | #include <algorithm>
 | 
|---|
| [3237b2] | 45 | #include <boost/math/quaternion.hpp>
 | 
|---|
| [d468b5] | 46 | #include <cmath>
 | 
|---|
| [601115] | 47 | #include <functional>
 | 
|---|
 | 48 | #include <iterator>
 | 
|---|
| [d468b5] | 49 | #include <limits>
 | 
|---|
 | 50 | #include <list>
 | 
|---|
| [f54930] | 51 | #include <vector>
 | 
|---|
| [d468b5] | 52 | #include <map>
 | 
|---|
| [f54930] | 53 | 
 | 
|---|
 | 54 | #include "LinearAlgebra/Line.hpp"
 | 
|---|
| [6cf5bb] | 55 | #include "LinearAlgebra/Plane.hpp"
 | 
|---|
| [f54930] | 56 | #include "LinearAlgebra/RealSpaceMatrix.hpp"
 | 
|---|
 | 57 | #include "LinearAlgebra/Vector.hpp"
 | 
|---|
 | 58 | 
 | 
|---|
| [6cf5bb] | 59 | // static entities
 | 
|---|
| [81557e] | 60 | const double SphericalPointDistribution::SQRT_3(sqrt(3.0));
 | 
|---|
| [6cf5bb] | 61 | const double SphericalPointDistribution::warn_amplitude = 1e-2;
 | 
|---|
 | 62 | 
 | 
|---|
 | 63 | typedef std::vector<double> DistanceArray_t;
 | 
|---|
| [81557e] | 64 | 
 | 
|---|
| [6cf5bb] | 65 | inline
 | 
|---|
| [d468b5] | 66 | DistanceArray_t calculatePairwiseDistances(
 | 
|---|
| [6cf5bb] | 67 |     const std::vector<Vector> &_points,
 | 
|---|
 | 68 |     const SphericalPointDistribution::IndexList_t &_indices
 | 
|---|
| [d468b5] | 69 |     )
 | 
|---|
 | 70 | {
 | 
|---|
 | 71 |   DistanceArray_t result;
 | 
|---|
| [6cf5bb] | 72 |   for (SphericalPointDistribution::IndexList_t::const_iterator firstiter = _indices.begin();
 | 
|---|
| [d468b5] | 73 |       firstiter != _indices.end(); ++firstiter) {
 | 
|---|
| [6cf5bb] | 74 |     for (SphericalPointDistribution::IndexList_t::const_iterator seconditer = firstiter;
 | 
|---|
| [d468b5] | 75 |         seconditer != _indices.end(); ++seconditer) {
 | 
|---|
 | 76 |       if (firstiter == seconditer)
 | 
|---|
 | 77 |         continue;
 | 
|---|
| [6cf5bb] | 78 |       const double distance = (_points[*firstiter] - _points[*seconditer]).NormSquared();
 | 
|---|
| [d468b5] | 79 |       result.push_back(distance);
 | 
|---|
 | 80 |     }
 | 
|---|
 | 81 |   }
 | 
|---|
 | 82 |   return result;
 | 
|---|
 | 83 | }
 | 
|---|
 | 84 | 
 | 
|---|
 | 85 | // class generator: taken from www.cplusplus.com example std::generate
 | 
|---|
 | 86 | struct c_unique {
 | 
|---|
 | 87 |   int current;
 | 
|---|
 | 88 |   c_unique() {current=0;}
 | 
|---|
| [601115] | 89 |   int operator()() {return current++;}
 | 
|---|
| [d468b5] | 90 | } UniqueNumber;
 | 
|---|
 | 91 | 
 | 
|---|
 | 92 | /** Returns squared L2 error of the given \a _Matching.
 | 
|---|
 | 93 |  *
 | 
|---|
 | 94 |  * We compare the pair-wise distances of each associated matching
 | 
|---|
 | 95 |  * and check whether these distances each match between \a _old and
 | 
|---|
 | 96 |  * \a _new.
 | 
|---|
 | 97 |  *
 | 
|---|
| [81557e] | 98 |  * \param _old first set of returnpolygon (fewer or equal to \a _new)
 | 
|---|
 | 99 |  * \param _new second set of returnpolygon
 | 
|---|
| [d468b5] | 100 |  * \param _Matching matching between the two sets
 | 
|---|
 | 101 |  * \return pair with L1 and squared L2 error
 | 
|---|
 | 102 |  */
 | 
|---|
| [6cf5bb] | 103 | std::pair<double, double> SphericalPointDistribution::calculateErrorOfMatching(
 | 
|---|
| [d468b5] | 104 |     const std::vector<Vector> &_old,
 | 
|---|
 | 105 |     const std::vector<Vector> &_new,
 | 
|---|
 | 106 |     const IndexList_t &_Matching)
 | 
|---|
 | 107 | {
 | 
|---|
 | 108 |   std::pair<double, double> errors( std::make_pair( 0., 0. ) );
 | 
|---|
 | 109 | 
 | 
|---|
 | 110 |   if (_Matching.size() > 1) {
 | 
|---|
| [5be870] | 111 |     LOG(3, "INFO: Matching is " << _Matching);
 | 
|---|
| [d468b5] | 112 | 
 | 
|---|
 | 113 |     // calculate all pair-wise distances
 | 
|---|
 | 114 |     IndexList_t keys(_Matching.size());
 | 
|---|
 | 115 |     std::generate (keys.begin(), keys.end(), UniqueNumber);
 | 
|---|
 | 116 |     const DistanceArray_t firstdistances = calculatePairwiseDistances(_old, keys);
 | 
|---|
 | 117 |     const DistanceArray_t seconddistances = calculatePairwiseDistances(_new, _Matching);
 | 
|---|
 | 118 | 
 | 
|---|
 | 119 |     ASSERT( firstdistances.size() == seconddistances.size(),
 | 
|---|
 | 120 |         "calculateL2ErrorOfMatching() - mismatch in pair-wise distance array sizes.");
 | 
|---|
 | 121 |     DistanceArray_t::const_iterator firstiter = firstdistances.begin();
 | 
|---|
 | 122 |     DistanceArray_t::const_iterator seconditer = seconddistances.begin();
 | 
|---|
 | 123 |     for (;(firstiter != firstdistances.end()) && (seconditer != seconddistances.end());
 | 
|---|
 | 124 |         ++firstiter, ++seconditer) {
 | 
|---|
 | 125 |       const double gap = *firstiter - *seconditer;
 | 
|---|
 | 126 |       // L1 error
 | 
|---|
 | 127 |       if (errors.first < gap)
 | 
|---|
 | 128 |         errors.first = gap;
 | 
|---|
 | 129 |       // L2 error
 | 
|---|
 | 130 |       errors.second += gap*gap;
 | 
|---|
 | 131 |     }
 | 
|---|
| [5be870] | 132 |   } else
 | 
|---|
| [601115] | 133 |     ELOG(3, "calculateErrorOfMatching() - Given matching's size is less than 2.");
 | 
|---|
| [5be870] | 134 |   LOG(3, "INFO: Resulting errors for matching (L1, L2): "
 | 
|---|
 | 135 |       << errors.first << "," << errors.second << ".");
 | 
|---|
| [d468b5] | 136 | 
 | 
|---|
 | 137 |   return errors;
 | 
|---|
 | 138 | }
 | 
|---|
 | 139 | 
 | 
|---|
| [6cf5bb] | 140 | SphericalPointDistribution::Polygon_t SphericalPointDistribution::removeMatchingPoints(
 | 
|---|
| [a4d335] | 141 |     const VectorArray_t &_points,
 | 
|---|
| [d468b5] | 142 |     const IndexList_t &_matchingindices
 | 
|---|
 | 143 |     )
 | 
|---|
 | 144 | {
 | 
|---|
| [81557e] | 145 |   SphericalPointDistribution::Polygon_t remainingreturnpolygon;
 | 
|---|
| [d468b5] | 146 |   IndexArray_t indices(_matchingindices.begin(), _matchingindices.end());
 | 
|---|
 | 147 |   std::sort(indices.begin(), indices.end());
 | 
|---|
| [5be870] | 148 |   LOG(4, "DEBUG: sorted matching is " << indices);
 | 
|---|
| [a4d335] | 149 |   IndexArray_t remainingindices(_points.size(), -1);
 | 
|---|
 | 150 |   std::generate(remainingindices.begin(), remainingindices.end(), UniqueNumber);
 | 
|---|
 | 151 |   IndexArray_t::iterator remainiter = std::set_difference(
 | 
|---|
 | 152 |       remainingindices.begin(), remainingindices.end(),
 | 
|---|
 | 153 |       indices.begin(), indices.end(),
 | 
|---|
 | 154 |       remainingindices.begin());
 | 
|---|
 | 155 |   remainingindices.erase(remainiter, remainingindices.end());
 | 
|---|
 | 156 |   LOG(4, "DEBUG: remaining indices are " << remainingindices);
 | 
|---|
 | 157 |   for (IndexArray_t::const_iterator iter = remainingindices.begin();
 | 
|---|
 | 158 |       iter != remainingindices.end(); ++iter) {
 | 
|---|
 | 159 |           remainingreturnpolygon.push_back(_points[*iter]);
 | 
|---|
| [d468b5] | 160 |   }
 | 
|---|
 | 161 | 
 | 
|---|
| [81557e] | 162 |   return remainingreturnpolygon;
 | 
|---|
| [d468b5] | 163 | }
 | 
|---|
 | 164 | 
 | 
|---|
 | 165 | /** Recursive function to go through all possible matchings.
 | 
|---|
 | 166 |  *
 | 
|---|
 | 167 |  * \param _MCS structure holding global information to the recursion
 | 
|---|
 | 168 |  * \param _matching current matching being build up
 | 
|---|
 | 169 |  * \param _indices contains still available indices
 | 
|---|
 | 170 |  * \param _matchingsize
 | 
|---|
 | 171 |  */
 | 
|---|
| [6cf5bb] | 172 | void SphericalPointDistribution::recurseMatchings(
 | 
|---|
| [d468b5] | 173 |     MatchingControlStructure &_MCS,
 | 
|---|
 | 174 |     IndexList_t &_matching,
 | 
|---|
 | 175 |     IndexList_t _indices,
 | 
|---|
 | 176 |     unsigned int _matchingsize)
 | 
|---|
| [f54930] | 177 | {
 | 
|---|
| [5be870] | 178 |   LOG(4, "DEBUG: Recursing with current matching " << _matching
 | 
|---|
 | 179 |       << ", remaining indices " << _indices
 | 
|---|
| [601115] | 180 |       << ", and sought size " << _matching.size()+_matchingsize);
 | 
|---|
| [d468b5] | 181 |   //!> threshold for L1 error below which matching is immediately acceptable
 | 
|---|
 | 182 |   const double L1THRESHOLD = 1e-2;
 | 
|---|
 | 183 |   if (!_MCS.foundflag) {
 | 
|---|
| [601115] | 184 |     LOG(4, "DEBUG: Current matching has size " << _matching.size() << ", places left " << _matchingsize);
 | 
|---|
 | 185 |     if (_matchingsize > 0) {
 | 
|---|
| [d468b5] | 186 |       // go through all indices
 | 
|---|
 | 187 |       for (IndexList_t::iterator iter = _indices.begin();
 | 
|---|
| [601115] | 188 |           (iter != _indices.end()) && (!_MCS.foundflag);) {
 | 
|---|
| [d468b5] | 189 |         // add index to matching
 | 
|---|
 | 190 |         _matching.push_back(*iter);
 | 
|---|
| [601115] | 191 |         LOG(5, "DEBUG: Adding " << *iter << " to matching.");
 | 
|---|
| [d468b5] | 192 |         // remove index but keep iterator to position (is the next to erase element)
 | 
|---|
 | 193 |         IndexList_t::iterator backupiter = _indices.erase(iter);
 | 
|---|
 | 194 |         // recurse with decreased _matchingsize
 | 
|---|
 | 195 |         recurseMatchings(_MCS, _matching, _indices, _matchingsize-1);
 | 
|---|
 | 196 |         // re-add chosen index and reset index to new position
 | 
|---|
 | 197 |         _indices.insert(backupiter, _matching.back());
 | 
|---|
 | 198 |         iter = backupiter;
 | 
|---|
 | 199 |         // remove index from _matching to make space for the next one
 | 
|---|
 | 200 |         _matching.pop_back();
 | 
|---|
 | 201 |       }
 | 
|---|
 | 202 |       // gone through all indices then exit recursion
 | 
|---|
| [601115] | 203 |       if (_matching.empty())
 | 
|---|
 | 204 |         _MCS.foundflag = true;
 | 
|---|
| [d468b5] | 205 |     } else {
 | 
|---|
| [5be870] | 206 |       LOG(3, "INFO: Found matching " << _matching);
 | 
|---|
| [d468b5] | 207 |       // calculate errors
 | 
|---|
 | 208 |       std::pair<double, double> errors = calculateErrorOfMatching(
 | 
|---|
| [6cf5bb] | 209 |           _MCS.oldpoints, _MCS.newpoints, _matching);
 | 
|---|
| [d468b5] | 210 |       if (errors.first < L1THRESHOLD) {
 | 
|---|
 | 211 |         _MCS.bestmatching = _matching;
 | 
|---|
 | 212 |         _MCS.foundflag = true;
 | 
|---|
| [601115] | 213 |       } else if (_MCS.bestL2 > errors.second) {
 | 
|---|
| [d468b5] | 214 |         _MCS.bestmatching = _matching;
 | 
|---|
 | 215 |         _MCS.bestL2 = errors.second;
 | 
|---|
 | 216 |       }
 | 
|---|
 | 217 |     }
 | 
|---|
| [f54930] | 218 |   }
 | 
|---|
 | 219 | }
 | 
|---|
 | 220 | 
 | 
|---|
| [6cf5bb] | 221 | /** Decides by an orthonormal third vector whether the sign of the rotation
 | 
|---|
 | 222 |  * angle should be negative or positive.
 | 
|---|
| [601115] | 223 |  *
 | 
|---|
| [6cf5bb] | 224 |  * \return -1 or 1
 | 
|---|
| [601115] | 225 |  */
 | 
|---|
| [6cf5bb] | 226 | inline
 | 
|---|
 | 227 | double determineSignOfRotation(
 | 
|---|
 | 228 |     const Vector &_oldPosition,
 | 
|---|
 | 229 |     const Vector &_newPosition,
 | 
|---|
 | 230 |     const Vector &_RotationAxis
 | 
|---|
 | 231 |     )
 | 
|---|
 | 232 | {
 | 
|---|
 | 233 |   Vector dreiBein(_oldPosition);
 | 
|---|
 | 234 |   dreiBein.VectorProduct(_RotationAxis);
 | 
|---|
 | 235 |   dreiBein.Normalize();
 | 
|---|
 | 236 |   const double sign =
 | 
|---|
 | 237 |       (dreiBein.ScalarProduct(_newPosition) < 0.) ? -1. : +1.;
 | 
|---|
 | 238 |   LOG(6, "DEBUG: oldCenter on plane is " << _oldPosition
 | 
|---|
 | 239 |       << ", newCenter in plane is " << _newPosition
 | 
|---|
 | 240 |       << ", and dreiBein is " << dreiBein);
 | 
|---|
 | 241 |   return sign;
 | 
|---|
 | 242 | }
 | 
|---|
 | 243 | 
 | 
|---|
 | 244 | /** Finds combinatorially the best matching between points in \a _polygon
 | 
|---|
 | 245 |  * and \a _newpolygon.
 | 
|---|
 | 246 |  *
 | 
|---|
 | 247 |  * We find the matching with the smallest L2 error, where we break when we stumble
 | 
|---|
 | 248 |  * upon a matching with zero error.
 | 
|---|
 | 249 |  *
 | 
|---|
 | 250 |  * \sa recurseMatchings() for going through all matchings
 | 
|---|
 | 251 |  *
 | 
|---|
 | 252 |  * \param _polygon here, we have indices 0,1,2,...
 | 
|---|
 | 253 |  * \param _newpolygon and here we need to find the correct indices
 | 
|---|
 | 254 |  * \return list of indices: first in \a _polygon goes to first index for \a _newpolygon
 | 
|---|
 | 255 |  */
 | 
|---|
 | 256 | SphericalPointDistribution::IndexList_t SphericalPointDistribution::findBestMatching(
 | 
|---|
| [601115] | 257 |     const SphericalPointDistribution::Polygon_t &_polygon,
 | 
|---|
| [6cf5bb] | 258 |     const SphericalPointDistribution::Polygon_t &_newpolygon
 | 
|---|
 | 259 |     )
 | 
|---|
| [601115] | 260 | {
 | 
|---|
| [6cf5bb] | 261 |   MatchingControlStructure MCS;
 | 
|---|
 | 262 |   MCS.foundflag = false;
 | 
|---|
 | 263 |   MCS.bestL2 = std::numeric_limits<double>::max();
 | 
|---|
 | 264 |   MCS.oldpoints.insert(MCS.oldpoints.begin(), _polygon.begin(),_polygon.end() );
 | 
|---|
 | 265 |   MCS.newpoints.insert(MCS.newpoints.begin(), _newpolygon.begin(),_newpolygon.end() );
 | 
|---|
 | 266 | 
 | 
|---|
 | 267 |   // search for bestmatching combinatorially
 | 
|---|
 | 268 |   {
 | 
|---|
 | 269 |     // translate polygon into vector to enable index addressing
 | 
|---|
 | 270 |     IndexList_t indices(_newpolygon.size());
 | 
|---|
 | 271 |     std::generate(indices.begin(), indices.end(), UniqueNumber);
 | 
|---|
 | 272 |     IndexList_t matching;
 | 
|---|
 | 273 | 
 | 
|---|
 | 274 |     // walk through all matchings
 | 
|---|
 | 275 |     const unsigned int matchingsize = _polygon.size();
 | 
|---|
 | 276 |     ASSERT( matchingsize <= indices.size(),
 | 
|---|
 | 277 |         "SphericalPointDistribution::matchSphericalPointDistributions() - not enough new points to choose for matching to old ones.");
 | 
|---|
 | 278 |     recurseMatchings(MCS, matching, indices, matchingsize);
 | 
|---|
| [601115] | 279 |   }
 | 
|---|
| [6cf5bb] | 280 |   return MCS.bestmatching;
 | 
|---|
 | 281 | }
 | 
|---|
 | 282 | 
 | 
|---|
 | 283 | inline
 | 
|---|
 | 284 | Vector calculateCenter(
 | 
|---|
 | 285 |     const SphericalPointDistribution::VectorArray_t &_positions,
 | 
|---|
 | 286 |     const SphericalPointDistribution::IndexList_t &_indices)
 | 
|---|
 | 287 | {
 | 
|---|
 | 288 |   Vector Center;
 | 
|---|
 | 289 |   Center.Zero();
 | 
|---|
 | 290 |   for (SphericalPointDistribution::IndexList_t::const_iterator iter = _indices.begin();
 | 
|---|
 | 291 |       iter != _indices.end(); ++iter)
 | 
|---|
 | 292 |     Center += _positions[*iter];
 | 
|---|
 | 293 |   if (!_indices.empty())
 | 
|---|
 | 294 |     Center *= 1./(double)_indices.size();
 | 
|---|
 | 295 | 
 | 
|---|
 | 296 |   return Center;
 | 
|---|
 | 297 | }
 | 
|---|
| [601115] | 298 | 
 | 
|---|
| [6cf5bb] | 299 | inline
 | 
|---|
 | 300 | void calculateOldAndNewCenters(
 | 
|---|
 | 301 |     Vector &_oldCenter,
 | 
|---|
 | 302 |     Vector &_newCenter,
 | 
|---|
 | 303 |     const SphericalPointDistribution::VectorArray_t &_referencepositions,
 | 
|---|
 | 304 |     const SphericalPointDistribution::VectorArray_t &_currentpositions,
 | 
|---|
 | 305 |     const SphericalPointDistribution::IndexList_t &_bestmatching)
 | 
|---|
 | 306 | {
 | 
|---|
 | 307 |   const size_t NumberIds = std::min(_bestmatching.size(), (size_t)3);
 | 
|---|
 | 308 |   SphericalPointDistribution::IndexList_t continuousIds(NumberIds, -1);
 | 
|---|
 | 309 |   std::generate(continuousIds.begin(), continuousIds.end(), UniqueNumber);
 | 
|---|
 | 310 |   _oldCenter = calculateCenter(_referencepositions, continuousIds);
 | 
|---|
 | 311 |   // C++11 defines a copy_n function ...
 | 
|---|
 | 312 |   SphericalPointDistribution::IndexList_t::const_iterator enditer = _bestmatching.begin();
 | 
|---|
 | 313 |   std::advance(enditer, NumberIds);
 | 
|---|
 | 314 |   SphericalPointDistribution::IndexList_t firstbestmatchingIds(NumberIds, -1);
 | 
|---|
 | 315 |   std::copy(_bestmatching.begin(), enditer, firstbestmatchingIds.begin());
 | 
|---|
 | 316 |   _newCenter = calculateCenter( _currentpositions, firstbestmatchingIds);
 | 
|---|
 | 317 | }
 | 
|---|
 | 318 | 
 | 
|---|
 | 319 | SphericalPointDistribution::Rotation_t SphericalPointDistribution::findPlaneAligningRotation(
 | 
|---|
 | 320 |     const VectorArray_t &_referencepositions,
 | 
|---|
 | 321 |     const VectorArray_t &_currentpositions,
 | 
|---|
 | 322 |     const IndexList_t &_bestmatching
 | 
|---|
 | 323 |     )
 | 
|---|
 | 324 | {
 | 
|---|
 | 325 |   bool dontcheck = false;
 | 
|---|
 | 326 |   // initialize to no rotation
 | 
|---|
 | 327 |   Rotation_t Rotation;
 | 
|---|
 | 328 |   Rotation.first.Zero();
 | 
|---|
 | 329 |   Rotation.first[0] = 1.;
 | 
|---|
 | 330 |   Rotation.second = 0.;
 | 
|---|
 | 331 | 
 | 
|---|
 | 332 |   // calculate center of triangle/line/point consisting of first points of matching
 | 
|---|
 | 333 |   Vector oldCenter;
 | 
|---|
 | 334 |   Vector newCenter;
 | 
|---|
 | 335 |   calculateOldAndNewCenters(
 | 
|---|
 | 336 |       oldCenter, newCenter,
 | 
|---|
 | 337 |       _referencepositions, _currentpositions, _bestmatching);
 | 
|---|
 | 338 | 
 | 
|---|
 | 339 |   if ((!oldCenter.IsZero()) && (!newCenter.IsZero())) {
 | 
|---|
 | 340 |     LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
 | 
|---|
 | 341 |     oldCenter.Normalize();
 | 
|---|
 | 342 |     newCenter.Normalize();
 | 
|---|
 | 343 |     if (!oldCenter.IsEqualTo(newCenter)) {
 | 
|---|
 | 344 |       // calculate rotation axis and angle
 | 
|---|
 | 345 |       Rotation.first = oldCenter;
 | 
|---|
 | 346 |       Rotation.first.VectorProduct(newCenter);
 | 
|---|
 | 347 |       Rotation.second = oldCenter.Angle(newCenter); // /(M_PI/2.);
 | 
|---|
 | 348 |     } else {
 | 
|---|
 | 349 |       // no rotation required anymore
 | 
|---|
 | 350 |     }
 | 
|---|
 | 351 |   } else {
 | 
|---|
 | 352 |     LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
 | 
|---|
 | 353 |     if ((oldCenter.IsZero()) && (newCenter.IsZero())) {
 | 
|---|
 | 354 |       // either oldCenter or newCenter (or both) is directly at origin
 | 
|---|
 | 355 |       if (_bestmatching.size() == 2) {
 | 
|---|
 | 356 |         // line case
 | 
|---|
 | 357 |         Vector oldPosition = _currentpositions[*_bestmatching.begin()];
 | 
|---|
 | 358 |         Vector newPosition = _referencepositions[0];
 | 
|---|
 | 359 |         // check whether we need to rotate at all
 | 
|---|
 | 360 |         if (!oldPosition.IsEqualTo(newPosition)) {
 | 
|---|
 | 361 |           Rotation.first = oldPosition;
 | 
|---|
 | 362 |           Rotation.first.VectorProduct(newPosition);
 | 
|---|
 | 363 |           // orientation will fix the sign here eventually
 | 
|---|
 | 364 |           Rotation.second = oldPosition.Angle(newPosition);
 | 
|---|
 | 365 |         } else {
 | 
|---|
 | 366 |           // no rotation required anymore
 | 
|---|
 | 367 |         }
 | 
|---|
 | 368 |       } else {
 | 
|---|
 | 369 |         // triangle case
 | 
|---|
 | 370 |         // both triangles/planes have same center, hence get axis by
 | 
|---|
 | 371 |         // VectorProduct of Normals
 | 
|---|
 | 372 |         Plane newplane(_referencepositions[0], _referencepositions[1], _referencepositions[2]);
 | 
|---|
 | 373 |         VectorArray_t vectors;
 | 
|---|
 | 374 |         for (IndexList_t::const_iterator iter = _bestmatching.begin();
 | 
|---|
 | 375 |             iter != _bestmatching.end(); ++iter)
 | 
|---|
 | 376 |           vectors.push_back(_currentpositions[*iter]);
 | 
|---|
 | 377 |         Plane oldplane(vectors[0], vectors[1], vectors[2]);
 | 
|---|
 | 378 |         Vector oldPosition = oldplane.getNormal();
 | 
|---|
 | 379 |         Vector newPosition = newplane.getNormal();
 | 
|---|
 | 380 |         // check whether we need to rotate at all
 | 
|---|
 | 381 |         if (!oldPosition.IsEqualTo(newPosition)) {
 | 
|---|
 | 382 |           Rotation.first = oldPosition;
 | 
|---|
 | 383 |           Rotation.first.VectorProduct(newPosition);
 | 
|---|
 | 384 |           Rotation.first.Normalize();
 | 
|---|
 | 385 | 
 | 
|---|
 | 386 |           // construct reference vector to determine direction of rotation
 | 
|---|
 | 387 |           const double sign = determineSignOfRotation(oldPosition, newPosition, Rotation.first);
 | 
|---|
 | 388 |           Rotation.second = sign * oldPosition.Angle(newPosition);
 | 
|---|
 | 389 |           LOG(5, "DEBUG: Rotating plane normals by " << Rotation.second
 | 
|---|
 | 390 |               << " around axis " << Rotation.first);
 | 
|---|
 | 391 |         } else {
 | 
|---|
 | 392 |           // else do nothing
 | 
|---|
 | 393 |         }
 | 
|---|
 | 394 |       }
 | 
|---|
 | 395 |     } else {
 | 
|---|
 | 396 |       // TODO: we can't do anything here, but this case needs to be dealt with when
 | 
|---|
 | 397 |       // we have no ideal geometries anymore
 | 
|---|
 | 398 |       if ((oldCenter-newCenter).Norm() > warn_amplitude)
 | 
|---|
 | 399 |         ELOG(2, "oldCenter is " << oldCenter << ", yet newCenter is " << newCenter);
 | 
|---|
 | 400 |       // else they are considered close enough
 | 
|---|
 | 401 |       dontcheck = true;
 | 
|---|
 | 402 |     }
 | 
|---|
 | 403 |   }
 | 
|---|
 | 404 | 
 | 
|---|
 | 405 | #ifndef NDEBUG
 | 
|---|
 | 406 |   // check: rotation brings newCenter onto oldCenter position
 | 
|---|
 | 407 |   if (!dontcheck) {
 | 
|---|
 | 408 |     Line Axis(zeroVec, Rotation.first);
 | 
|---|
 | 409 |     Vector test = Axis.rotateVector(newCenter, Rotation.second);
 | 
|---|
 | 410 |     LOG(4, "CHECK: rotated newCenter is " << test
 | 
|---|
 | 411 |         << ", oldCenter is " << oldCenter);
 | 
|---|
 | 412 |     ASSERT( (test - oldCenter).NormSquared() < std::numeric_limits<double>::epsilon()*1e4,
 | 
|---|
 | 413 |         "matchSphericalPointDistributions() - rotation does not work as expected by "
 | 
|---|
 | 414 |         +toString((test - oldCenter).NormSquared())+".");
 | 
|---|
 | 415 |   }
 | 
|---|
 | 416 | #endif
 | 
|---|
 | 417 | 
 | 
|---|
 | 418 |   return Rotation;
 | 
|---|
 | 419 | }
 | 
|---|
 | 420 | 
 | 
|---|
 | 421 | SphericalPointDistribution::Rotation_t SphericalPointDistribution::findPointAligningRotation(
 | 
|---|
 | 422 |     const VectorArray_t &remainingold,
 | 
|---|
 | 423 |     const VectorArray_t &remainingnew,
 | 
|---|
 | 424 |     const IndexList_t &_bestmatching)
 | 
|---|
 | 425 | {
 | 
|---|
 | 426 |   // initialize rotation to zero
 | 
|---|
 | 427 |   Rotation_t Rotation;
 | 
|---|
 | 428 |   Rotation.first.Zero();
 | 
|---|
 | 429 |   Rotation.first[0] = 1.;
 | 
|---|
 | 430 |   Rotation.second = 0.;
 | 
|---|
 | 431 | 
 | 
|---|
 | 432 |   // recalculate center
 | 
|---|
 | 433 |   Vector oldCenter;
 | 
|---|
 | 434 |   Vector newCenter;
 | 
|---|
 | 435 |   calculateOldAndNewCenters(
 | 
|---|
 | 436 |       oldCenter, newCenter,
 | 
|---|
 | 437 |       remainingold, remainingnew, _bestmatching);
 | 
|---|
 | 438 | 
 | 
|---|
 | 439 |   Vector oldPosition = remainingnew[*_bestmatching.begin()];
 | 
|---|
 | 440 |   Vector newPosition = remainingold[0];
 | 
|---|
 | 441 |   LOG(6, "DEBUG: oldPosition is " << oldPosition << " and newPosition is " << newPosition);
 | 
|---|
 | 442 |   if (!oldPosition.IsEqualTo(newPosition)) {
 | 
|---|
 | 443 |     if ((!oldCenter.IsZero()) && (!newCenter.IsZero())) {
 | 
|---|
 | 444 |       oldCenter.Normalize();  // note weighted sum of normalized weight is not normalized
 | 
|---|
 | 445 |       Rotation.first = oldCenter;
 | 
|---|
 | 446 |       LOG(6, "DEBUG: Picking normalized oldCenter as Rotation.first " << oldCenter);
 | 
|---|
 | 447 |       oldPosition.ProjectOntoPlane(Rotation.first);
 | 
|---|
 | 448 |       newPosition.ProjectOntoPlane(Rotation.first);
 | 
|---|
 | 449 |       LOG(6, "DEBUG: Positions after projection are " << oldPosition << " and " << newPosition);
 | 
|---|
 | 450 |     } else {
 | 
|---|
 | 451 |       if (_bestmatching.size() == 2) {
 | 
|---|
 | 452 |         // line situation
 | 
|---|
 | 453 |         try {
 | 
|---|
 | 454 |           Plane oldplane(oldPosition, oldCenter, newPosition);
 | 
|---|
 | 455 |           Rotation.first = oldplane.getNormal();
 | 
|---|
 | 456 |           LOG(6, "DEBUG: Plane is " << oldplane << " and normal is " << Rotation.first);
 | 
|---|
 | 457 |         } catch (LinearDependenceException &e) {
 | 
|---|
 | 458 |           LOG(6, "DEBUG: Vectors defining plane are linearly dependent.");
 | 
|---|
 | 459 |           // oldPosition and newPosition are on a line, just flip when not equal
 | 
|---|
 | 460 |           if (!oldPosition.IsEqualTo(newPosition)) {
 | 
|---|
 | 461 |             Rotation.first.Zero();
 | 
|---|
 | 462 |             Rotation.first.GetOneNormalVector(oldPosition);
 | 
|---|
 | 463 |             LOG(6, "DEBUG: For flipping we use Rotation.first " << Rotation.first);
 | 
|---|
 | 464 |             assert( Rotation.first.ScalarProduct(oldPosition) < std::numeric_limits<double>::epsilon()*1e4);
 | 
|---|
 | 465 |   //              Rotation.second = M_PI;
 | 
|---|
 | 466 |           } else {
 | 
|---|
 | 467 |             LOG(6, "DEBUG: oldPosition and newPosition are equivalent.");
 | 
|---|
 | 468 |           }
 | 
|---|
 | 469 |         }
 | 
|---|
 | 470 |       } else {
 | 
|---|
 | 471 |         // triangle situation
 | 
|---|
 | 472 |         Plane oldplane(remainingold[0], remainingold[1], remainingold[2]);
 | 
|---|
 | 473 |         Rotation.first = oldplane.getNormal();
 | 
|---|
 | 474 |         LOG(6, "DEBUG: oldPlane is " << oldplane << " and normal is " << Rotation.first);
 | 
|---|
 | 475 |         oldPosition.ProjectOntoPlane(Rotation.first);
 | 
|---|
 | 476 |         LOG(6, "DEBUG: Positions after projection are " << oldPosition << " and " << newPosition);
 | 
|---|
 | 477 |       }
 | 
|---|
 | 478 |     }
 | 
|---|
 | 479 |     // construct reference vector to determine direction of rotation
 | 
|---|
 | 480 |     const double sign = determineSignOfRotation(oldPosition, newPosition, Rotation.first);
 | 
|---|
 | 481 |     Rotation.second = sign * oldPosition.Angle(newPosition);
 | 
|---|
 | 482 |   } else {
 | 
|---|
 | 483 |     LOG(6, "DEBUG: oldPosition and newPosition are equivalent, hence no orientating rotation.");
 | 
|---|
 | 484 |   }
 | 
|---|
 | 485 | 
 | 
|---|
 | 486 |   return Rotation;
 | 
|---|
| [601115] | 487 | }
 | 
|---|
 | 488 | 
 | 
|---|
 | 489 | 
 | 
|---|
| [d468b5] | 490 | SphericalPointDistribution::Polygon_t
 | 
|---|
 | 491 | SphericalPointDistribution::matchSphericalPointDistributions(
 | 
|---|
 | 492 |     const SphericalPointDistribution::Polygon_t &_polygon,
 | 
|---|
 | 493 |     const SphericalPointDistribution::Polygon_t &_newpolygon
 | 
|---|
 | 494 |     )
 | 
|---|
 | 495 | {
 | 
|---|
| [81557e] | 496 |   SphericalPointDistribution::Polygon_t remainingreturnpolygon;
 | 
|---|
| [d468b5] | 497 |   VectorArray_t remainingold(_polygon.begin(), _polygon.end());
 | 
|---|
 | 498 |   VectorArray_t remainingnew(_newpolygon.begin(), _newpolygon.end());
 | 
|---|
| [601115] | 499 |   LOG(2, "INFO: Matching old polygon " << _polygon
 | 
|---|
| [5be870] | 500 |       << " with new polygon " << _newpolygon);
 | 
|---|
| [d468b5] | 501 | 
 | 
|---|
| [6cf5bb] | 502 |   if (_polygon.size() == _newpolygon.size()) {
 | 
|---|
 | 503 |     // same number of points desired as are present? Do nothing
 | 
|---|
 | 504 |     LOG(2, "INFO: There are no vacant points to return.");
 | 
|---|
 | 505 |     return remainingreturnpolygon;
 | 
|---|
 | 506 |   }
 | 
|---|
| [d468b5] | 507 | 
 | 
|---|
| [6cf5bb] | 508 |   if (_polygon.size() > 0) {
 | 
|---|
 | 509 |     IndexList_t bestmatching = findBestMatching(_polygon, _newpolygon);
 | 
|---|
 | 510 |     LOG(2, "INFO: Best matching is " << bestmatching);
 | 
|---|
| [d468b5] | 511 | 
 | 
|---|
 | 512 |     // determine rotation angles to align the two point distributions with
 | 
|---|
| [6cf5bb] | 513 |     // respect to bestmatching:
 | 
|---|
 | 514 |     // we use the center between the three first matching points
 | 
|---|
 | 515 |     /// the first rotation brings these two centers to coincide
 | 
|---|
| [a4d335] | 516 |     VectorArray_t rotated_newpolygon = remainingnew;
 | 
|---|
| [d468b5] | 517 |     {
 | 
|---|
| [6cf5bb] | 518 |       Rotation_t Rotation = findPlaneAligningRotation(
 | 
|---|
 | 519 |           remainingold,
 | 
|---|
 | 520 |           remainingnew,
 | 
|---|
 | 521 |           bestmatching);
 | 
|---|
 | 522 |       LOG(5, "DEBUG: Rotating coordinate system by " << Rotation.second
 | 
|---|
 | 523 |           << " around axis " << Rotation.first);
 | 
|---|
 | 524 |       Line Axis(zeroVec, Rotation.first);
 | 
|---|
 | 525 | 
 | 
|---|
 | 526 |       // apply rotation angle to bring newCenter to oldCenter
 | 
|---|
 | 527 |       for (VectorArray_t::iterator iter = rotated_newpolygon.begin();
 | 
|---|
 | 528 |           iter != rotated_newpolygon.end(); ++iter) {
 | 
|---|
 | 529 |         Vector ¤t = *iter;
 | 
|---|
 | 530 |         LOG(6, "DEBUG: Original point is " << current);
 | 
|---|
 | 531 |         current =  Axis.rotateVector(current, Rotation.second);
 | 
|---|
 | 532 |         LOG(6, "DEBUG: Rotated point is " << current);
 | 
|---|
| [d468b5] | 533 |       }
 | 
|---|
| [6cf5bb] | 534 | 
 | 
|---|
 | 535 | #ifndef NDEBUG
 | 
|---|
 | 536 |       // check: rotated "newCenter" should now equal oldCenter
 | 
|---|
 | 537 |       {
 | 
|---|
 | 538 |         Vector oldCenter;
 | 
|---|
 | 539 |         Vector rotatednewCenter;
 | 
|---|
 | 540 |         calculateOldAndNewCenters(
 | 
|---|
 | 541 |             oldCenter, rotatednewCenter,
 | 
|---|
 | 542 |             remainingold, rotated_newpolygon, bestmatching);
 | 
|---|
 | 543 |         // NOTE: Center must not necessarily lie on the sphere with norm 1, hence, we
 | 
|---|
 | 544 |         // have to normalize it just as before, as oldCenter and newCenter lengths may differ.
 | 
|---|
 | 545 |         if ((!oldCenter.IsZero()) && (!rotatednewCenter.IsZero())) {
 | 
|---|
 | 546 |           oldCenter.Normalize();
 | 
|---|
 | 547 |           rotatednewCenter.Normalize();
 | 
|---|
 | 548 |           LOG(4, "CHECK: rotatednewCenter is " << rotatednewCenter
 | 
|---|
 | 549 |               << ", oldCenter is " << oldCenter);
 | 
|---|
 | 550 |           ASSERT( (rotatednewCenter - oldCenter).NormSquared() < std::numeric_limits<double>::epsilon()*1e4,
 | 
|---|
 | 551 |               "matchSphericalPointDistributions() - rotation does not work as expected by "
 | 
|---|
 | 552 |               +toString((rotatednewCenter - oldCenter).NormSquared())+".");
 | 
|---|
| [a4d335] | 553 |         }
 | 
|---|
 | 554 |       }
 | 
|---|
| [6cf5bb] | 555 | #endif
 | 
|---|
| [a4d335] | 556 |     }
 | 
|---|
| [6cf5bb] | 557 |     /// the second (orientation) rotation aligns the planes such that the
 | 
|---|
 | 558 |     /// points themselves coincide
 | 
|---|
 | 559 |     if (bestmatching.size() > 1) {
 | 
|---|
 | 560 |       Rotation_t Rotation = findPointAligningRotation(
 | 
|---|
 | 561 |           remainingold,
 | 
|---|
 | 562 |           rotated_newpolygon,
 | 
|---|
 | 563 |           bestmatching);
 | 
|---|
 | 564 | 
 | 
|---|
 | 565 |       // construct RotationAxis and two points on its plane, defining the angle
 | 
|---|
 | 566 |       Rotation.first.Normalize();
 | 
|---|
 | 567 |       const Line RotationAxis(zeroVec, Rotation.first);
 | 
|---|
 | 568 | 
 | 
|---|
 | 569 |       LOG(5, "DEBUG: Rotating around self is " << Rotation.second
 | 
|---|
 | 570 |           << " around axis " << RotationAxis);
 | 
|---|
| [a4d335] | 571 | 
 | 
|---|
| [3237b2] | 572 | #ifndef NDEBUG
 | 
|---|
| [6cf5bb] | 573 |       // check: first bestmatching in rotated_newpolygon and remainingnew
 | 
|---|
 | 574 |       // should now equal
 | 
|---|
 | 575 |       {
 | 
|---|
 | 576 |         const IndexList_t::const_iterator iter = bestmatching.begin();
 | 
|---|
 | 577 |         Vector rotatednew = RotationAxis.rotateVector(
 | 
|---|
 | 578 |             rotated_newpolygon[*iter],
 | 
|---|
 | 579 |             Rotation.second);
 | 
|---|
 | 580 |         LOG(4, "CHECK: rotated first new bestmatching is " << rotatednew
 | 
|---|
 | 581 |             << " while old was " << remainingold[0]);
 | 
|---|
 | 582 |         ASSERT( (rotatednew - remainingold[0]).Norm() < warn_amplitude,
 | 
|---|
 | 583 |             "matchSphericalPointDistributions() - orientation rotation ends up off by more than "
 | 
|---|
 | 584 |             +toString(warn_amplitude)+".");
 | 
|---|
 | 585 |       }
 | 
|---|
| [3237b2] | 586 | #endif
 | 
|---|
| [601115] | 587 | 
 | 
|---|
| [6cf5bb] | 588 |       for (VectorArray_t::iterator iter = rotated_newpolygon.begin();
 | 
|---|
 | 589 |           iter != rotated_newpolygon.end(); ++iter) {
 | 
|---|
 | 590 |         Vector ¤t = *iter;
 | 
|---|
 | 591 |         LOG(6, "DEBUG: Original point is " << current);
 | 
|---|
 | 592 |         current = RotationAxis.rotateVector(current, Rotation.second);
 | 
|---|
 | 593 |         LOG(6, "DEBUG: Rotated point is " << current);
 | 
|---|
| [3237b2] | 594 |       }
 | 
|---|
 | 595 |     }
 | 
|---|
| [d468b5] | 596 | 
 | 
|---|
| [601115] | 597 |     // remove all points in matching and return remaining ones
 | 
|---|
 | 598 |     SphericalPointDistribution::Polygon_t remainingpoints =
 | 
|---|
| [6cf5bb] | 599 |         removeMatchingPoints(rotated_newpolygon, bestmatching);
 | 
|---|
| [601115] | 600 |     LOG(2, "INFO: Remaining points are " << remainingpoints);
 | 
|---|
 | 601 |     return remainingpoints;
 | 
|---|
| [d468b5] | 602 |   } else
 | 
|---|
 | 603 |     return _newpolygon;
 | 
|---|
 | 604 | }
 | 
|---|
| [3e52312] | 605 | 
 | 
|---|
 | 606 | SphericalPointDistribution::Polygon_t
 | 
|---|
 | 607 | SphericalPointDistribution::getSimplePolygon(const int _NumberOfPoints) const
 | 
|---|
 | 608 | {
 | 
|---|
 | 609 |   Polygon_t returnpolygon;
 | 
|---|
 | 610 | 
 | 
|---|
 | 611 |         switch (_NumberOfPoints)
 | 
|---|
 | 612 |         {
 | 
|---|
 | 613 |           case 0:
 | 
|---|
 | 614 |             returnpolygon = get<0>();
 | 
|---|
 | 615 |             break;
 | 
|---|
 | 616 |           case 1:
 | 
|---|
 | 617 |             returnpolygon = get<1>();
 | 
|---|
 | 618 |             break;
 | 
|---|
 | 619 |           case 2:
 | 
|---|
 | 620 |             returnpolygon = get<2>();
 | 
|---|
 | 621 |             break;
 | 
|---|
 | 622 |           case 3:
 | 
|---|
 | 623 |             returnpolygon = get<3>();
 | 
|---|
 | 624 |             break;
 | 
|---|
 | 625 |           case 4:
 | 
|---|
 | 626 |             returnpolygon = get<4>();
 | 
|---|
 | 627 |             break;
 | 
|---|
 | 628 |           case 5:
 | 
|---|
 | 629 |             returnpolygon = get<5>();
 | 
|---|
 | 630 |             break;
 | 
|---|
 | 631 |           case 6:
 | 
|---|
 | 632 |             returnpolygon = get<6>();
 | 
|---|
 | 633 |             break;
 | 
|---|
 | 634 |           case 7:
 | 
|---|
 | 635 |             returnpolygon = get<7>();
 | 
|---|
 | 636 |             break;
 | 
|---|
 | 637 |           case 8:
 | 
|---|
 | 638 |             returnpolygon = get<8>();
 | 
|---|
 | 639 |             break;
 | 
|---|
 | 640 |           case 9:
 | 
|---|
 | 641 |             returnpolygon = get<9>();
 | 
|---|
 | 642 |             break;
 | 
|---|
 | 643 |           case 10:
 | 
|---|
 | 644 |             returnpolygon = get<10>();
 | 
|---|
 | 645 |             break;
 | 
|---|
 | 646 |           case 11:
 | 
|---|
 | 647 |             returnpolygon = get<11>();
 | 
|---|
 | 648 |             break;
 | 
|---|
 | 649 |           case 12:
 | 
|---|
 | 650 |             returnpolygon = get<12>();
 | 
|---|
 | 651 |             break;
 | 
|---|
 | 652 |           case 14:
 | 
|---|
 | 653 |             returnpolygon = get<14>();
 | 
|---|
 | 654 |             break;
 | 
|---|
 | 655 |           default:
 | 
|---|
 | 656 |             ASSERT(0, "SphericalPointDistribution::initSelf() - cannot deal with the case "
 | 
|---|
 | 657 |                 +toString(_NumberOfPoints)+".");
 | 
|---|
 | 658 |         }
 | 
|---|
 | 659 | 
 | 
|---|
 | 660 |         return returnpolygon;
 | 
|---|
 | 661 | }
 | 
|---|
 | 662 | 
 | 
|---|