| [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) {
|
|---|
| [ab7199] | 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 |
|
|---|