source: src/Fragmentation/Exporters/SphericalPointDistribution.cpp@ 4b96da

Last change on this file since 4b96da was 4b96da, checked in by Frederik Heber <heber@…>, 10 years ago

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