source: src/Fragmentation/Exporters/SphericalPointDistribution.cpp@ 1cde4e8

Last change on this file since 1cde4e8 was 1cde4e8, checked in by Frederik Heber <heber@…>, 11 years ago

Refactoring SphericalPointDistribution to combine point list and subset of indices.
  • Property mode set to 100644
File size: 31.6 KB
Line 
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"
40#include "CodePatterns/IteratorAdaptors.hpp"
41#include "CodePatterns/Log.hpp"
42#include "CodePatterns/toString.hpp"
43
44#include <algorithm>
45#include <boost/math/quaternion.hpp>
46#include <cmath>
47#include <functional>
48#include <iterator>
49#include <limits>
50#include <list>
51#include <numeric>
52#include <vector>
53#include <map>
54
55#include "LinearAlgebra/Line.hpp"
56#include "LinearAlgebra/Plane.hpp"
57#include "LinearAlgebra/RealSpaceMatrix.hpp"
58#include "LinearAlgebra/Vector.hpp"
59
60// static entities
61const double SphericalPointDistribution::warn_amplitude = 1e-2;
62const double SphericalPointDistribution::L1THRESHOLD = 1e-2;
63const double SphericalPointDistribution::L2THRESHOLD = 2e-1;
64
65typedef std::vector<double> DistanceArray_t;
66
67// class generator: taken from www.cplusplus.com example std::generate
68struct c_unique {
69 unsigned int current;
70 c_unique() {current=0;}
71 unsigned int operator()() {return current++;}
72} UniqueNumber;
73
74struct c_unique_list {
75 unsigned int current;
76 c_unique_list() {current=0;}
77 std::list<unsigned int> operator()() {return std::list<unsigned int>(1, current++);}
78} UniqueNumberList;
79
80/** Calculate the center of a given set of points in \a _positions but only
81 * for those indicated by \a _indices.
82 *
83 */
84inline
85Vector calculateCenter(
86 const SphericalPointDistribution::VectorArray_t &_positions,
87 const SphericalPointDistribution::IndexList_t &_indices)
88{
89 Vector Center;
90 Center.Zero();
91 for (SphericalPointDistribution::IndexList_t::const_iterator iter = _indices.begin();
92 iter != _indices.end(); ++iter)
93 Center += _positions[*iter];
94 if (!_indices.empty())
95 Center *= 1./(double)_indices.size();
96
97 return Center;
98}
99
100/** Calculate the center of a given set of points in \a _positions but only
101 * for those indicated by \a _indices.
102 *
103 */
104inline
105Vector calculateCenter(
106 const SphericalPointDistribution::PolygonWithIndices &_polygon)
107{
108 return calculateCenter(_polygon.polygon, _polygon.indices);
109}
110
111inline
112DistanceArray_t calculatePairwiseDistances(
113 const SphericalPointDistribution::VectorArray_t &_points,
114 const SphericalPointDistribution::IndexTupleList_t &_indices
115 )
116{
117 DistanceArray_t result;
118 for (SphericalPointDistribution::IndexTupleList_t::const_iterator firstiter = _indices.begin();
119 firstiter != _indices.end(); ++firstiter) {
120
121 // calculate first center from possible tuple of indices
122 Vector FirstCenter;
123 ASSERT(!firstiter->empty(), "calculatePairwiseDistances() - there is an empty tuple.");
124 if (firstiter->size() == 1) {
125 FirstCenter = _points[*firstiter->begin()];
126 } else {
127 FirstCenter = calculateCenter( _points, *firstiter);
128 if (!FirstCenter.IsZero())
129 FirstCenter.Normalize();
130 }
131
132 for (SphericalPointDistribution::IndexTupleList_t::const_iterator seconditer = firstiter;
133 seconditer != _indices.end(); ++seconditer) {
134 if (firstiter == seconditer)
135 continue;
136
137 // calculate second center from possible tuple of indices
138 Vector SecondCenter;
139 ASSERT(!seconditer->empty(), "calculatePairwiseDistances() - there is an empty tuple.");
140 if (seconditer->size() == 1) {
141 SecondCenter = _points[*seconditer->begin()];
142 } else {
143 SecondCenter = calculateCenter( _points, *seconditer);
144 if (!SecondCenter.IsZero())
145 SecondCenter.Normalize();
146 }
147
148 // calculate distance between both centers
149 double distance = 2.; // greatest distance on surface of sphere with radius 1.
150 if ((!FirstCenter.IsZero()) && (!SecondCenter.IsZero()))
151 distance = (FirstCenter - SecondCenter).NormSquared();
152 result.push_back(distance);
153 }
154 }
155 return result;
156}
157
158/** Decides by an orthonormal third vector whether the sign of the rotation
159 * angle should be negative or positive.
160 *
161 * \return -1 or 1
162 */
163inline
164double determineSignOfRotation(
165 const Vector &_oldPosition,
166 const Vector &_newPosition,
167 const Vector &_RotationAxis
168 )
169{
170 Vector dreiBein(_oldPosition);
171 dreiBein.VectorProduct(_RotationAxis);
172 ASSERT( !dreiBein.IsZero(), "determineSignOfRotation() - dreiBein is zero.");
173 dreiBein.Normalize();
174 const double sign =
175 (dreiBein.ScalarProduct(_newPosition) < 0.) ? -1. : +1.;
176 LOG(6, "DEBUG: oldCenter on plane is " << _oldPosition
177 << ", newCenter on plane is " << _newPosition
178 << ", and dreiBein is " << dreiBein);
179 return sign;
180}
181
182/** Convenience function to recalculate old and new center for determining plane
183 * rotation.
184 */
185inline
186void calculateOldAndNewCenters(
187 Vector &_oldCenter,
188 Vector &_newCenter,
189 const SphericalPointDistribution::PolygonWithIndices &_referencepositions,
190 const SphericalPointDistribution::PolygonWithIndices &_currentpositions)
191{
192 _oldCenter = calculateCenter(_referencepositions.polygon, _referencepositions.indices);
193 // C++11 defines a copy_n function ...
194 _newCenter = calculateCenter( _currentpositions.polygon, _currentpositions.indices);
195}
196/** Returns squared L2 error of the given \a _Matching.
197 *
198 * We compare the pair-wise distances of each associated matching
199 * and check whether these distances each match between \a _old and
200 * \a _new.
201 *
202 * \param _old first set of points (fewer or equal to \a _new)
203 * \param _new second set of points
204 * \param _Matching matching between the two sets
205 * \return pair with L1 and squared L2 error
206 */
207std::pair<double, double> SphericalPointDistribution::calculateErrorOfMatching(
208 const VectorArray_t &_old,
209 const VectorArray_t &_new,
210 const IndexTupleList_t &_Matching)
211{
212 std::pair<double, double> errors( std::make_pair( 0., 0. ) );
213
214 if (_Matching.size() > 1) {
215 LOG(5, "INFO: Matching is " << _Matching);
216
217 // calculate all pair-wise distances
218 IndexTupleList_t keys(_old.size(), IndexList_t() );
219 std::generate (keys.begin(), keys.end(), UniqueNumberList);
220
221 const DistanceArray_t firstdistances = calculatePairwiseDistances(_old, keys);
222 const DistanceArray_t seconddistances = calculatePairwiseDistances(_new, _Matching);
223
224 ASSERT( firstdistances.size() == seconddistances.size(),
225 "calculateL2ErrorOfMatching() - mismatch in pair-wise distance array sizes.");
226 DistanceArray_t::const_iterator firstiter = firstdistances.begin();
227 DistanceArray_t::const_iterator seconditer = seconddistances.begin();
228 for (;(firstiter != firstdistances.end()) && (seconditer != seconddistances.end());
229 ++firstiter, ++seconditer) {
230 const double gap = fabs(*firstiter - *seconditer);
231 // L1 error
232 if (errors.first < gap)
233 errors.first = gap;
234 // L2 error
235 errors.second += gap*gap;
236 }
237 } else {
238 // check whether we have any zero centers: Combining points on new sphere may result
239 // in zero centers
240 for (SphericalPointDistribution::IndexTupleList_t::const_iterator iter = _Matching.begin();
241 iter != _Matching.end(); ++iter) {
242 if ((iter->size() != 1) && (calculateCenter( _new, *iter).IsZero())) {
243 errors.first = 2.;
244 errors.second = 2.;
245 }
246 }
247 }
248 LOG(4, "INFO: Resulting errors for matching (L1, L2): "
249 << errors.first << "," << errors.second << ".");
250
251 return errors;
252}
253
254SphericalPointDistribution::Polygon_t SphericalPointDistribution::removeMatchingPoints(
255 const PolygonWithIndices &_points
256 )
257{
258 SphericalPointDistribution::Polygon_t remainingpoints;
259 IndexArray_t indices(_points.indices.begin(), _points.indices.end());
260 std::sort(indices.begin(), indices.end());
261 LOG(4, "DEBUG: sorted matching is " << indices);
262 IndexArray_t remainingindices(_points.polygon.size(), -1);
263 std::generate(remainingindices.begin(), remainingindices.end(), UniqueNumber);
264 IndexArray_t::iterator remainiter = std::set_difference(
265 remainingindices.begin(), remainingindices.end(),
266 indices.begin(), indices.end(),
267 remainingindices.begin());
268 remainingindices.erase(remainiter, remainingindices.end());
269 LOG(4, "DEBUG: remaining indices are " << remainingindices);
270 for (IndexArray_t::const_iterator iter = remainingindices.begin();
271 iter != remainingindices.end(); ++iter) {
272 remainingpoints.push_back(_points.polygon[*iter]);
273 }
274
275 return remainingpoints;
276}
277
278/** Recursive function to go through all possible matchings.
279 *
280 * \param _MCS structure holding global information to the recursion
281 * \param _matching current matching being build up
282 * \param _indices contains still available indices
283 * \param _remainingweights current weights to fill (each weight a place)
284 * \param _remainiter iterator over the weights, indicating the current position we match
285 * \param _matchingsize
286 */
287void SphericalPointDistribution::recurseMatchings(
288 MatchingControlStructure &_MCS,
289 IndexTupleList_t &_matching,
290 IndexList_t _indices,
291 WeightsArray_t &_remainingweights,
292 WeightsArray_t::iterator _remainiter,
293 const unsigned int _matchingsize
294 )
295{
296 LOG(5, "DEBUG: Recursing with current matching " << _matching
297 << ", remaining indices " << _indices
298 << ", and remaining weights " << _matchingsize);
299 if (!_MCS.foundflag) {
300 LOG(5, "DEBUG: Current matching has size " << _matching.size() << ", places left " << _matchingsize);
301 if (_matchingsize > 0) {
302 // go through all indices
303 for (IndexList_t::iterator iter = _indices.begin();
304 (iter != _indices.end()) && (!_MCS.foundflag);) {
305 // check whether we can stay in the current bin or have to move on to next one
306 if (*_remainiter == 0) {
307 // we need to move on
308 if (_remainiter != _remainingweights.end()) {
309 ++_remainiter;
310 } else {
311 // as we check _matchingsize > 0 this should be impossible
312 ASSERT( 0, "recurseMatchings() - we must not come to this position.");
313 }
314 }
315 // advance in matching to same position
316 const size_t OldIndex = std::distance(_remainingweights.begin(), _remainiter);
317 while (_matching.size() <= OldIndex) { // add empty lists of new bin is opened
318 LOG(6, "DEBUG: Extending _matching.");
319 _matching.push_back( IndexList_t() );
320 }
321 IndexTupleList_t::iterator filliniter = _matching.begin();
322 std::advance(filliniter, OldIndex);
323 // add index to matching
324 filliniter->push_back(*iter);
325 --(*_remainiter);
326 LOG(6, "DEBUG: Adding " << *iter << " to matching at " << OldIndex << ".");
327 // remove index but keep iterator to position (is the next to erase element)
328 IndexList_t::iterator backupiter = _indices.erase(iter);
329 // recurse with decreased _matchingsize
330 recurseMatchings(_MCS, _matching, _indices, _remainingweights, _remainiter, _matchingsize-1);
331 // re-add chosen index and reset index to new position
332 _indices.insert(backupiter, filliniter->back());
333 iter = backupiter;
334 // remove index from _matching to make space for the next one
335 filliniter->pop_back();
336 ++(*_remainiter);
337 }
338 // gone through all indices then exit recursion
339 if (_matching.empty())
340 _MCS.foundflag = true;
341 } else {
342 LOG(4, "INFO: Found matching " << _matching);
343 // calculate errors
344 std::pair<double, double> errors = calculateErrorOfMatching(
345 _MCS.oldpoints, _MCS.newpoints, _matching);
346 if (errors.first < L1THRESHOLD) {
347 _MCS.bestmatching = _matching;
348 _MCS.foundflag = true;
349 } else if (_MCS.bestL2 > errors.second) {
350 _MCS.bestmatching = _matching;
351 _MCS.bestL2 = errors.second;
352 }
353 }
354 }
355}
356
357/** Finds combinatorially the best matching between points in \a _polygon
358 * and \a _newpolygon.
359 *
360 * We find the matching with the smallest L2 error, where we break when we stumble
361 * upon a matching with zero error.
362 *
363 * As points in \a _polygon may be have a weight greater 1 we have to match it to
364 * multiple points in \a _newpolygon. Eventually, these multiple points are combined
365 * for their center of weight, which is the only thing follow-up function see of
366 * these multiple points. Hence, we actually modify \a _newpolygon in the process
367 * such that the returned IndexList_t indicates a bijective mapping in the end.
368 *
369 * \sa recurseMatchings() for going through all matchings
370 *
371 * \param _polygon here, we have indices 0,1,2,...
372 * \param _newpolygon and here we need to find the correct indices
373 * \return list of indices: first in \a _polygon goes to first index for \a _newpolygon
374 */
375SphericalPointDistribution::IndexList_t SphericalPointDistribution::findBestMatching(
376 const WeightedPolygon_t &_polygon,
377 Polygon_t &_newpolygon
378 )
379{
380 MatchingControlStructure MCS;
381 MCS.foundflag = false;
382 MCS.bestL2 = std::numeric_limits<double>::max();
383 // transform lists into arrays
384 for (WeightedPolygon_t::const_iterator iter = _polygon.begin();
385 iter != _polygon.end(); ++iter) {
386 MCS.oldpoints.push_back(iter->first);
387 MCS.weights.push_back(iter->second);
388 }
389 MCS.newpoints.insert(MCS.newpoints.begin(), _newpolygon.begin(),_newpolygon.end() );
390
391 // search for bestmatching combinatorially
392 {
393 // translate polygon into vector to enable index addressing
394 IndexList_t indices(_newpolygon.size());
395 std::generate(indices.begin(), indices.end(), UniqueNumber);
396 IndexTupleList_t matching;
397
398 // walk through all matchings
399 WeightsArray_t remainingweights = MCS.weights;
400 unsigned int placesleft = std::accumulate(remainingweights.begin(), remainingweights.end(), 0);
401 recurseMatchings(MCS, matching, indices, remainingweights, remainingweights.begin(), placesleft);
402 }
403 if (MCS.foundflag)
404 LOG(3, "Found a best matching beneath L1 threshold of " << L1THRESHOLD);
405 else {
406 if (MCS.bestL2 < warn_amplitude)
407 LOG(3, "Picking matching is " << MCS.bestmatching << " with best L2 error of "
408 << MCS.bestL2);
409 else if (MCS.bestL2 < L2THRESHOLD)
410 ELOG(2, "Picking matching is " << MCS.bestmatching
411 << " with rather large L2 error of " << MCS.bestL2);
412 else
413 ASSERT(0, "findBestMatching() - matching "+toString(MCS.bestmatching)
414 +" has L2 error of "+toString(MCS.bestL2)+" that is too large.");
415 }
416
417 // combine multiple points and create simple IndexList from IndexTupleList
418 const SphericalPointDistribution::IndexList_t IndexList =
419 joinPoints(_newpolygon, MCS.newpoints, MCS.bestmatching);
420
421 return IndexList;
422}
423
424SphericalPointDistribution::IndexList_t SphericalPointDistribution::joinPoints(
425 Polygon_t &_newpolygon,
426 const VectorArray_t &_newpoints,
427 const IndexTupleList_t &_bestmatching
428 )
429{
430 // combine all multiple points
431 IndexList_t IndexList;
432 IndexArray_t removalpoints;
433 unsigned int UniqueIndex = _newpolygon.size(); // all indices up to size are used right now
434 VectorArray_t newCenters;
435 newCenters.reserve(_bestmatching.size());
436 for (IndexTupleList_t::const_iterator tupleiter = _bestmatching.begin();
437 tupleiter != _bestmatching.end(); ++tupleiter) {
438 ASSERT (tupleiter->size() > 0,
439 "findBestMatching() - encountered tuple in bestmatching with size 0.");
440 if (tupleiter->size() == 1) {
441 // add point and index
442 IndexList.push_back(*tupleiter->begin());
443 } else {
444 // combine into weighted and normalized center
445 Vector Center = calculateCenter(_newpoints, *tupleiter);
446 Center.Normalize();
447 _newpolygon.push_back(Center);
448 LOG(5, "DEBUG: Combining " << tupleiter->size() << " points to weighted center "
449 << Center << " with new index " << UniqueIndex);
450 // mark for removal
451 removalpoints.insert(removalpoints.end(), tupleiter->begin(), tupleiter->end());
452 // add new index
453 IndexList.push_back(UniqueIndex++);
454 }
455 }
456 // IndexList is now our new bestmatching (that is bijective)
457 LOG(4, "DEBUG: Our new bijective IndexList reads as " << IndexList);
458
459 // modifying _newpolygon: remove all points in removalpoints, add those in newCenters
460 Polygon_t allnewpoints = _newpolygon;
461 {
462 _newpolygon.clear();
463 std::sort(removalpoints.begin(), removalpoints.end());
464 size_t i = 0;
465 IndexArray_t::const_iterator removeiter = removalpoints.begin();
466 for (Polygon_t::iterator iter = allnewpoints.begin();
467 iter != allnewpoints.end(); ++iter, ++i) {
468 if ((removeiter != removalpoints.end()) && (i == *removeiter)) {
469 // don't add, go to next remove index
470 ++removeiter;
471 } else {
472 // otherwise add points
473 _newpolygon.push_back(*iter);
474 }
475 }
476 }
477 LOG(4, "DEBUG: The polygon with recentered points removed is " << _newpolygon);
478
479 // map IndexList to new shrinked _newpolygon
480 typedef std::set<unsigned int> IndexSet_t;
481 IndexSet_t SortedIndexList(IndexList.begin(), IndexList.end());
482 IndexList.clear();
483 {
484 size_t offset = 0;
485 IndexSet_t::const_iterator listiter = SortedIndexList.begin();
486 IndexArray_t::const_iterator removeiter = removalpoints.begin();
487 for (size_t i = 0; i < allnewpoints.size(); ++i) {
488 if ((removeiter != removalpoints.end()) && (i == *removeiter)) {
489 ++offset;
490 ++removeiter;
491 } else if ((listiter != SortedIndexList.end()) && (i == *listiter)) {
492 IndexList.push_back(*listiter - offset);
493 ++listiter;
494 }
495 }
496 }
497 LOG(4, "DEBUG: Our new bijective IndexList corrected for removed points reads as "
498 << IndexList);
499
500 return IndexList;
501}
502
503SphericalPointDistribution::Rotation_t SphericalPointDistribution::findPlaneAligningRotation(
504 const PolygonWithIndices &_referencepositions,
505 const PolygonWithIndices &_currentpositions
506 )
507{
508 bool dontcheck = false;
509 // initialize to no rotation
510 Rotation_t Rotation;
511 Rotation.first.Zero();
512 Rotation.first[0] = 1.;
513 Rotation.second = 0.;
514
515 // calculate center of triangle/line/point consisting of first points of matching
516 Vector oldCenter;
517 Vector newCenter;
518 calculateOldAndNewCenters(
519 oldCenter, newCenter,
520 _referencepositions, _currentpositions);
521
522 if ((!oldCenter.IsZero()) && (!newCenter.IsZero())) {
523 LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
524 oldCenter.Normalize();
525 newCenter.Normalize();
526 if (!oldCenter.IsEqualTo(newCenter)) {
527 // calculate rotation axis and angle
528 Rotation.first = oldCenter;
529 Rotation.first.VectorProduct(newCenter);
530 Rotation.second = oldCenter.Angle(newCenter); // /(M_PI/2.);
531 } else {
532 // no rotation required anymore
533 }
534 } else {
535 LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
536 if ((oldCenter.IsZero()) && (newCenter.IsZero())) {
537 // either oldCenter or newCenter (or both) is directly at origin
538 if (_currentpositions.indices.size() == 2) {
539 // line case
540 Vector oldPosition = _currentpositions.polygon[*_currentpositions.indices.begin()];
541 Vector newPosition = _referencepositions.polygon[0];
542 // check whether we need to rotate at all
543 if (!oldPosition.IsEqualTo(newPosition)) {
544 Rotation.first = oldPosition;
545 Rotation.first.VectorProduct(newPosition);
546 // orientation will fix the sign here eventually
547 Rotation.second = oldPosition.Angle(newPosition);
548 } else {
549 // no rotation required anymore
550 }
551 } else {
552 // triangle case
553 // both triangles/planes have same center, hence get axis by
554 // VectorProduct of Normals
555 Plane newplane(_referencepositions.polygon[0], _referencepositions.polygon[1], _referencepositions.polygon[2]);
556 VectorArray_t vectors;
557 for (IndexList_t::const_iterator iter = _currentpositions.indices.begin();
558 iter != _currentpositions.indices.end(); ++iter)
559 vectors.push_back(_currentpositions.polygon[*iter]);
560 Plane oldplane(vectors[0], vectors[1], vectors[2]);
561 Vector oldPosition = oldplane.getNormal();
562 Vector newPosition = newplane.getNormal();
563 // check whether we need to rotate at all
564 if (!oldPosition.IsEqualTo(newPosition)) {
565 Rotation.first = oldPosition;
566 Rotation.first.VectorProduct(newPosition);
567 Rotation.first.Normalize();
568
569 // construct reference vector to determine direction of rotation
570 const double sign = determineSignOfRotation(oldPosition, newPosition, Rotation.first);
571 Rotation.second = sign * oldPosition.Angle(newPosition);
572 LOG(5, "DEBUG: Rotating plane normals by " << Rotation.second
573 << " around axis " << Rotation.first);
574 } else {
575 // else do nothing
576 }
577 }
578 } else {
579 // TODO: we can't do anything here, but this case needs to be dealt with when
580 // we have no ideal geometries anymore
581 if ((oldCenter-newCenter).Norm() > warn_amplitude)
582 ELOG(2, "oldCenter is " << oldCenter << ", yet newCenter is " << newCenter);
583 // else they are considered close enough
584 dontcheck = true;
585 }
586 }
587
588#ifndef NDEBUG
589 // check: rotation brings newCenter onto oldCenter position
590 if (!dontcheck) {
591 Line Axis(zeroVec, Rotation.first);
592 Vector test = Axis.rotateVector(newCenter, Rotation.second);
593 LOG(4, "CHECK: rotated newCenter is " << test
594 << ", oldCenter is " << oldCenter);
595 ASSERT( (test - oldCenter).NormSquared() < std::numeric_limits<double>::epsilon()*1e4,
596 "matchSphericalPointDistributions() - rotation does not work as expected by "
597 +toString((test - oldCenter).NormSquared())+".");
598 }
599#endif
600
601 return Rotation;
602}
603
604SphericalPointDistribution::Rotation_t SphericalPointDistribution::findPointAligningRotation(
605 const PolygonWithIndices &remainingold,
606 const PolygonWithIndices &remainingnew)
607{
608 // initialize rotation to zero
609 Rotation_t Rotation;
610 Rotation.first.Zero();
611 Rotation.first[0] = 1.;
612 Rotation.second = 0.;
613
614 // recalculate center
615 Vector oldCenter;
616 Vector newCenter;
617 calculateOldAndNewCenters(
618 oldCenter, newCenter,
619 remainingold, remainingnew);
620
621 Vector oldPosition = remainingnew.polygon[*remainingnew.indices.begin()];
622 Vector newPosition = remainingold.polygon[0];
623 LOG(6, "DEBUG: oldPosition is " << oldPosition << " (length: "
624 << oldPosition.Norm() << ") and newPosition is " << newPosition << " length(: "
625 << newPosition.Norm() << ")");
626 if (!oldPosition.IsEqualTo(newPosition)) {
627 if ((!oldCenter.IsZero()) && (!newCenter.IsZero())) {
628 // we rotate at oldCenter and around the radial direction, which is again given
629 // by oldCenter.
630 Rotation.first = oldCenter;
631 Rotation.first.Normalize(); // note weighted sum of normalized weight is not normalized
632 LOG(6, "DEBUG: Using oldCenter " << oldCenter << " as rotation center and "
633 << Rotation.first << " as axis.");
634 oldPosition -= oldCenter;
635 newPosition -= oldCenter;
636 oldPosition = (oldPosition - oldPosition.Projection(Rotation.first));
637 newPosition = (newPosition - newPosition.Projection(Rotation.first));
638 LOG(6, "DEBUG: Positions after projection are " << oldPosition << " and " << newPosition);
639 } else {
640 if (remainingnew.indices.size() == 2) {
641 // line situation
642 try {
643 Plane oldplane(oldPosition, oldCenter, newPosition);
644 Rotation.first = oldplane.getNormal();
645 LOG(6, "DEBUG: Plane is " << oldplane << " and normal is " << Rotation.first);
646 } catch (LinearDependenceException &e) {
647 LOG(6, "DEBUG: Vectors defining plane are linearly dependent.");
648 // oldPosition and newPosition are on a line, just flip when not equal
649 if (!oldPosition.IsEqualTo(newPosition)) {
650 Rotation.first.Zero();
651 Rotation.first.GetOneNormalVector(oldPosition);
652 LOG(6, "DEBUG: For flipping we use Rotation.first " << Rotation.first);
653 assert( Rotation.first.ScalarProduct(oldPosition) < std::numeric_limits<double>::epsilon()*1e4);
654 // Rotation.second = M_PI;
655 } else {
656 LOG(6, "DEBUG: oldPosition and newPosition are equivalent.");
657 }
658 }
659 } else {
660 // triangle situation
661 Plane oldplane(remainingold.polygon[0], remainingold.polygon[1], remainingold.polygon[2]);
662 Rotation.first = oldplane.getNormal();
663 LOG(6, "DEBUG: oldPlane is " << oldplane << " and normal is " << Rotation.first);
664 oldPosition.ProjectOntoPlane(Rotation.first);
665 LOG(6, "DEBUG: Positions after projection are " << oldPosition << " and " << newPosition);
666 }
667 }
668 // construct reference vector to determine direction of rotation
669 const double sign = determineSignOfRotation(oldPosition, newPosition, Rotation.first);
670 Rotation.second = sign * oldPosition.Angle(newPosition);
671 } else {
672 LOG(6, "DEBUG: oldPosition and newPosition are equivalent, hence no orientating rotation.");
673 }
674
675 return Rotation;
676}
677
678
679SphericalPointDistribution::Polygon_t
680SphericalPointDistribution::matchSphericalPointDistributions(
681 const SphericalPointDistribution::WeightedPolygon_t &_polygon,
682 SphericalPointDistribution::Polygon_t &_newpolygon
683 )
684{
685 SphericalPointDistribution::Polygon_t remainingpoints;
686
687 LOG(2, "INFO: Matching old polygon " << _polygon
688 << " with new polygon " << _newpolygon);
689
690 if (_polygon.size() == _newpolygon.size()) {
691 // same number of points desired as are present? Do nothing
692 LOG(2, "INFO: There are no vacant points to return.");
693 return remainingpoints;
694 }
695
696 if (_polygon.size() > 0) {
697 IndexList_t bestmatching = findBestMatching(_polygon, _newpolygon);
698 LOG(2, "INFO: Best matching is " << bestmatching);
699
700 const size_t NumberIds = std::min(bestmatching.size(), (size_t)3);
701 // create old set
702 PolygonWithIndices oldSet;
703 oldSet.indices.resize(NumberIds, -1);
704 std::generate(oldSet.indices.begin(), oldSet.indices.end(), UniqueNumber);
705 for (WeightedPolygon_t::const_iterator iter = _polygon.begin();
706 iter != _polygon.end(); ++iter)
707 oldSet.polygon.push_back(iter->first);
708
709 // _newpolygon has changed, so now convert to array with matched indices
710 PolygonWithIndices newSet;
711 SphericalPointDistribution::IndexList_t::const_iterator beginiter = bestmatching.begin();
712 SphericalPointDistribution::IndexList_t::const_iterator enditer = bestmatching.begin();
713 std::advance(enditer, NumberIds);
714 newSet.indices.resize(NumberIds, -1);
715 std::copy(beginiter, enditer, newSet.indices.begin());
716 std::copy(_newpolygon.begin(),_newpolygon.end(), std::back_inserter(newSet.polygon));
717
718 // determine rotation angles to align the two point distributions with
719 // respect to bestmatching:
720 // we use the center between the three first matching points
721 /// the first rotation brings these two centers to coincide
722 PolygonWithIndices rotatednewSet = newSet;
723 {
724 Rotation_t Rotation = findPlaneAligningRotation(oldSet, newSet);
725 LOG(5, "DEBUG: Rotating coordinate system by " << Rotation.second
726 << " around axis " << Rotation.first);
727 Line Axis(zeroVec, Rotation.first);
728
729 // apply rotation angle to bring newCenter to oldCenter
730 for (VectorArray_t::iterator iter = rotatednewSet.polygon.begin();
731 iter != rotatednewSet.polygon.end(); ++iter) {
732 Vector &current = *iter;
733 LOG(6, "DEBUG: Original point is " << current);
734 current = Axis.rotateVector(current, Rotation.second);
735 LOG(6, "DEBUG: Rotated point is " << current);
736 }
737
738#ifndef NDEBUG
739 // check: rotated "newCenter" should now equal oldCenter
740 {
741 Vector oldCenter;
742 Vector rotatednewCenter;
743 calculateOldAndNewCenters(
744 oldCenter, rotatednewCenter,
745 oldSet, rotatednewSet);
746 // NOTE: Center must not necessarily lie on the sphere with norm 1, hence, we
747 // have to normalize it just as before, as oldCenter and newCenter lengths may differ.
748 if ((!oldCenter.IsZero()) && (!rotatednewCenter.IsZero())) {
749 oldCenter.Normalize();
750 rotatednewCenter.Normalize();
751 LOG(4, "CHECK: rotatednewCenter is " << rotatednewCenter
752 << ", oldCenter is " << oldCenter);
753 const double difference = (rotatednewCenter - oldCenter).NormSquared();
754 ASSERT( difference < std::numeric_limits<double>::epsilon()*1e4,
755 "matchSphericalPointDistributions() - rotation does not work as expected by "
756 +toString(difference)+".");
757 }
758 }
759#endif
760 }
761 /// the second (orientation) rotation aligns the planes such that the
762 /// points themselves coincide
763 if (bestmatching.size() > 1) {
764 Rotation_t Rotation = findPointAligningRotation(oldSet, rotatednewSet);
765
766 // construct RotationAxis and two points on its plane, defining the angle
767 Rotation.first.Normalize();
768 const Line RotationAxis(zeroVec, Rotation.first);
769
770 LOG(5, "DEBUG: Rotating around self is " << Rotation.second
771 << " around axis " << RotationAxis);
772
773#ifndef NDEBUG
774 // check: first bestmatching in rotated_newpolygon and remainingnew
775 // should now equal
776 {
777 const IndexList_t::const_iterator iter = bestmatching.begin();
778
779 // check whether both old and newPosition are at same distance to oldCenter
780 Vector oldCenter = calculateCenter(oldSet);
781 const double distance = fabs(
782 (oldSet.polygon[0] - oldCenter).NormSquared()
783 - (rotatednewSet.polygon[*iter] - oldCenter).NormSquared()
784 );
785 LOG(4, "CHECK: Squared distance between oldPosition and newPosition "
786 << " with respect to oldCenter " << oldCenter << " is " << distance);
787// ASSERT( distance < warn_amplitude,
788// "matchSphericalPointDistributions() - old and newPosition's squared distance to oldCenter differs by "
789// +toString(distance));
790
791 Vector rotatednew = RotationAxis.rotateVector(
792 rotatednewSet.polygon[*iter],
793 Rotation.second);
794 LOG(4, "CHECK: rotated first new bestmatching is " << rotatednew
795 << " while old was " << oldSet.polygon[0]);
796 const double difference = (rotatednew - oldSet.polygon[0]).NormSquared();
797 ASSERT( difference < distance+1e-8,
798 "matchSphericalPointDistributions() - orientation rotation ends up off by "
799 +toString(difference)+", more than "
800 +toString(distance+1e-8)+".");
801 }
802#endif
803
804 for (VectorArray_t::iterator iter = rotatednewSet.polygon.begin();
805 iter != rotatednewSet.polygon.end(); ++iter) {
806 Vector &current = *iter;
807 LOG(6, "DEBUG: Original point is " << current);
808 current = RotationAxis.rotateVector(current, Rotation.second);
809 LOG(6, "DEBUG: Rotated point is " << current);
810 }
811 }
812
813 // remove all points in matching and return remaining ones
814 SphericalPointDistribution::Polygon_t remainingpoints =
815 removeMatchingPoints(rotatednewSet);
816 LOG(2, "INFO: Remaining points are " << remainingpoints);
817 return remainingpoints;
818 } else
819 return _newpolygon;
820}
821
822
Note: See TracBrowser for help on using the repository browser.