Context Navigation

-              r8d38e6
+              rf9d85f
 #include <algorithm>
+#include <boost/math/quaternion.hpp>
 #include <cmath>
 #include <functional>
 …
+}
-/** Convert cartesian to polar coordinates.
+ *
- * \param _cartesian vector in cartesian coordinates
- * \return vector containing \f$ (r,\theta, \varphi \f$ tuple for polar coordinates
- */
-std::vector<double> getPolarCoordinates(const Vector &_cartesian)
+{
-  std::vector<double> polar(3,0.);
-  const double xsqr = _cartesian[0] * _cartesian[0];
-  const double ysqr = _cartesian[1] * _cartesian[1];
-  polar[0] = sqrt(xsqr + ysqr + _cartesian[2]*_cartesian[2]);
-  if (fabs(_cartesian[2]) < std::numeric_limits<double>::epsilon()*1e4) {
-    if (fabs(xsqr + ysqr) < std::numeric_limits<double>::epsilon()*1e4) {
-      polar[1] = 0.;
-    } else {
-      // xsqr + ysqr is always non-negative
-      polar[1] = M_PI/2.;
+    }
-  } else {
-    polar[1] = atan( sqrt(xsqr + ysqr)/_cartesian[2]);
-    if (_cartesian[2] <= -std::numeric_limits<double>::epsilon()*1e4)
-      polar[1] += M_PI;
+  }
-  if (fabs(_cartesian[0]) < std::numeric_limits<double>::epsilon()*1e4) {
-    if (fabs(_cartesian[1]) < std::numeric_limits<double>::epsilon()*1e4) {
-      polar[2] = 0.;
-    } else if (_cartesian[1] > std::numeric_limits<double>::epsilon()*1e4) {
-      polar[2] = M_PI/2.;
-    } else {
-      polar[2] = -M_PI/2.;
+    }
-  } else {
-    polar[2] = atan ( _cartesian[1]/_cartesian[0] );
-    if (_cartesian[0] <= -std::numeric_limits<double>::epsilon()*1e4)
-      polar[2] += M_PI;
+  }
-  return polar;
+}
-/** Calculate cartesian coordinates from given polar ones.
+ *
- * \param _polar vector with polar coordinates
- * \return cartesian coordinates
- */
-Vector getCartesianCoordinates(const std::vector<double> &_polar)
+{
-  Vector cartesian;
-  ASSERT( _polar.size() == 3,
-      "convertToCartesianCoordinates() - tuples has insufficient components.");
-  cartesian[0] = _polar[0] * sin(_polar[1]) * cos(_polar[2]);
-  cartesian[1] = _polar[0] * sin(_polar[1]) * sin(_polar[2]);
-  cartesian[2] = _polar[0] * cos(_polar[1]);
-  return cartesian;
+}
 /** Rotates a given polygon around x, y, and z axis by the given angles.
+ *
  * \param _polygon polygon whose points to rotate
  * \param _angles vector with rotation angles for x,y,z axis
+ * \param _q quaternion specifying the rotation of the coordinate system
  */
 SphericalPointDistribution::Polygon_t rotatePolygon(
     const SphericalPointDistribution::Polygon_t &_polygon,
     const std::vector<double> &_angles)
+    const boost::math::quaternion<double> &_q)
+{
   SphericalPointDistribution::Polygon_t rotated_polygon = _polygon;
+  RealSpaceMatrix rotation;
+  ASSERT( _angles.size() == 3,
+      "rotatePolygon() - not exactly "+toString(3)+" components given.");
+  boost::math::quaternion<double> q_inverse =
+      boost::math::conj(_q)/(boost::math::norm(_q));
   // apply rotation angles
   for (SphericalPointDistribution::Polygon_t::iterator iter = rotated_polygon.begin();
       iter != rotated_polygon.end(); ++iter) {
+    // transform to polar
+    std::vector<double> polar = getPolarCoordinates(*iter);
+    LOG(5, "DEBUG: Converting " << *iter << " to " << polar);
+    // sum up difference
+    std::transform(
+        _angles.begin(), _angles.end(),
+        polar.begin(),
+        polar.begin(),
+        std::plus<double>());
+    // convert back
+    *iter = getCartesianCoordinates(polar);
+    LOG(5, "DEBUG: Converting modified " << polar << " to " << *iter);
+    Vector &current = *iter;
+    boost::math::quaternion<double> p(0, current[0], current[1], current[2]);
+    p = _q * p * q_inverse;
+    LOG(5, "DEBUG: Rotated point is " << p);
+    // i have no idea why but first component comes up with wrong sign
+    current[0] = -p.R_component_2();
+    current[1] = p.R_component_3();
+    current[2] = p.R_component_4();
+  }
 …
     // determine rotation angles to align the two point distributions with
     // respect to bestmatching
+    std::vector<double> angles(NDIM);
+    Vector newCenter;
+    SphericalPointDistribution::Polygon_t rotated_newpolygon;
     Vector oldCenter;
+    {
       // calculate center of triangle/line/point consisting of first points of matching
+      Vector newCenter;
       IndexList_t::const_iterator iter = MCS.bestmatching.begin();
       unsigned int i = 0;
 …
       LOG(4, "DEBUG: oldCenter is " << oldCenter << ", newCenter is " << newCenter);
+      // transform to polar coordinates and note difference in angular parts
+      std::vector<double> oldpolar = getPolarCoordinates(oldCenter);
+      std::vector<double> newpolar = getPolarCoordinates(newCenter);
+      std::vector<double> differencepolar;
+      std::transform(
+          oldpolar.begin(), oldpolar.end(),
+          newpolar.begin(),
+          std::back_inserter(differencepolar),
+          std::minus<double>());
+      LOG(3, "INFO: (r,theta,phi) angles are" << differencepolar);
+    }
+    // rotate _newpolygon
+    SphericalPointDistribution::Polygon_t rotated_newpolygon =
+        rotatePolygon(_newpolygon, angles);
+    LOG(5, "DEBUG: Rotated new polygon is " << rotated_newpolygon);
+      if ((oldCenter - newCenter).NormSquared() > std::numeric_limits<double>::epsilon()*1e4) {
+        // setup quaternion
+        Vector RotationAxis = newCenter;
+        RotationAxis.VectorProduct(oldCenter);
+        RotationAxis.Normalize();
+        const double RotationAngle = oldCenter.Angle(newCenter)/(M_PI/2.);
+//            RotationAxis.Angle(oldCenter) - RotationAxis.Angle(newCenter);
+        boost::math::quaternion<double> q
+            (RotationAngle, RotationAxis[0], RotationAxis[1], RotationAxis[2]);
+        LOG(5, "DEBUG: RotationAxis is " << RotationAxis
+            << ", RotationAngle is " << RotationAngle);
+        LOG(5, "DEBUG: Quaternion describing rotation is " << q);
+#ifndef NDEBUG
+        {
+        // check that rotation works in DEBUG mode
+        boost::math::quaternion<double> p(0., newCenter[0], newCenter[1], newCenter[2]);
+        boost::math::quaternion<double> q_inverse =
+            boost::math::conj(q)/(boost::math::norm(q));
+        p = q * p * q_inverse;
+        boost::math::quaternion<double> identity(1,0,0,0);
+        ASSERT( boost::math::norm(q*q_inverse - identity) < std::numeric_limits<double>::epsilon()*1e4,
+            "matchSphericalPointDistributions() - quaternion does not rotate newCenter "
+            +toString(q*q_inverse)+" into oldCenter "+toString(identity)+".");
+        boost::math::quaternion<double> comparison(0., -oldCenter[0], oldCenter[1], oldCenter[2]);
+        ASSERT( boost::math::norm(p - comparison) < std::numeric_limits<double>::epsilon()*1e4,
+            "matchSphericalPointDistributions() - quaternion does not rotate newCenter "
+            +toString(p)+" into oldCenter "+toString(comparison)+".");
+        }
+#endif
+        // rotate spherical distribution
+        rotated_newpolygon = rotatePolygon(_newpolygon, q);
+        LOG(5, "DEBUG: Rotated new polygon is " << rotated_newpolygon);
+      } else {
+        rotated_newpolygon = _newpolygon;
+      }
+    }
+    // rotate triangle/line/point around itself to match orientation
     const Line RotationAxis(zeroVec, oldCenter);
     const double RotationAngle =

Note: See TracChangeset for help on using the changeset viewer.

Context Navigation

Changeset f9d85f for src/Fragmentation/Exporters/SphericalPointDistribution.cpp

Legend:

src/Fragmentation/Exporters/SphericalPointDistribution.cpp

Download in other formats: