rotation.hh

#ifndef ROTATION_HH
#define ROTATION_HH

/** \file
    \brief Define rotations in Euclidean spaces
*/

#include <array>

#include <dune/common/fvector.hh>
#include <dune/common/fmatrix.hh>
#include <dune/common/exceptions.hh>
#include <dune/common/math.hh>

#include "quaternion.hh"
#include <dune/gfe/tensor3.hh>
#include <dune/gfe/unitvector.hh>
#include <dune/gfe/skewmatrix.hh>
#include <dune/gfe/symmetricmatrix.hh>

template <class T, int dim>
class Rotation
{

};

/** \brief Specialization for dim==2
    \tparam T The type used for coordinates
*/
template <class T>
class Rotation<T,2>
{
public:
    /** \brief The type used for coordinates */
    typedef T ctype;

    /** \brief Dimension of the manifold formed by the 2d rotations */
    static const int dim = 1;

    /** \brief Coordinates are embedded in the euclidean space */
    static const int embeddedDim = 1;

    /** \brief Member of the corresponding Lie algebra.  This really is a skew-symmetric matrix */
    typedef Dune::FieldVector<T,1> TangentVector;

    /** \brief Member of the corresponding Lie algebra.  This really is a skew-symmetric matrix

    This vector is not really embedded in anything.  I have to make my notation more consistent! */
    typedef Dune::FieldVector<T,1> EmbeddedTangentVector;

    /** \brief The global convexity radius of the rotation group */
    static constexpr double convexityRadius = 0.5 * M_PI;

    /** \brief Default constructor, create the identity rotation */
    Rotation()
        : angle_(0)
    {}

    Rotation(const T& angle)
        : angle_(angle)
    {}

    /** \brief Return the identity element */
    static Rotation<T,2> identity() {
        // Default constructor creates an identity
        Rotation<T,2> id;
        return id;
    }

    static T distance(const Rotation<T,2>& a, const Rotation<T,2>& b) {
        T dist = a.angle_ - b.angle_;
        while (dist < 0)
            dist += 2*M_PI;
        while (dist > 2*M_PI)
            dist -= 2*M_PI;

        return (dist <= M_PI) ? dist : 2*M_PI - dist;
    }

    /** \brief The exponential map from a given point $p \in SO(3)$. */
    static Rotation<T,2> exp(const Rotation<T,2>& p, const TangentVector& v) {
        Rotation<T,2> result = p;
        result.angle_ += v;
        return result;
    }

    /** \brief The exponential map from \f$ \mathfrak{so}(2) \f$ to \f$ SO(2) \f$
     */
    static Rotation<T,2> exp(const Dune::FieldVector<T,1>& v) {
        Rotation<T,2> result;
        result.angle_ = v[0];
        return result;
    }

    static TangentVector derivativeOfDistanceSquaredWRTSecondArgument(const Rotation<T,2>& a,
                                                                      const Rotation<T,2>& b) {
        // This assertion is here to remind me of the following laziness:
        // The difference has to be computed modulo 2\pi
        assert( std::abs(a.angle_ - b.angle_) <= M_PI );
        return -2 * (a.angle_ - b.angle_);
    }

    static Dune::FieldMatrix<T,1,1> secondDerivativeOfDistanceSquaredWRTSecondArgument(const Rotation<T,2>& a,
                                                                                            const Rotation<T,2>& b) {
        return 2;
    }

    /** \brief Right multiplication */
    Rotation<T,2> mult(const Rotation<T,2>& other) const {
        Rotation<T,2> q = *this;
        q.angle_ += other.angle_;
        return q;
    }

    /** \brief Compute an orthonormal basis of the tangent space of SO(3).

    This basis is of course not globally continuous.
    */
    Dune::FieldMatrix<T,1,1> orthonormalFrame() const {
        return Dune::FieldMatrix<T,1,1>(1);
    }

    //private:

    // We store the rotation as an angle
    T angle_;
};

//! Send configuration to output stream
template <class T>
std::ostream& operator<< (std::ostream& s, const Rotation<T,2>& c)
  {
      return s << "[" << c.angle_ << "  (" << std::sin(c.angle_) << " " << std::cos(c.angle_) << ") ]";
  }


/** \brief Specialization for dim==3

Uses unit quaternion coordinates.
\todo Reimplement the method inverse() such that it returns a Rotation instead of a Quaternion.
Then remove the cast in the method setRotation, file averageinterface.hh
*/
template <class T>
class Rotation<T,3> : public Quaternion<T>
{

    /** \brief Computes sin(x/2) / x without getting unstable for small x */
    static T sincHalf(const T& x) {
        using std::sin;
        return (x < 1e-1) ? 0.5 - (x*x/48)  + Dune::power(x,4)/3840 - Dune::power(x,6)/645120  : sin(x/2)/x;
    }

    /** \brief Computes sin(sqrt(x)/2) / sqrt(x) without getting unstable for small x
     *
     * Using this somewhat exotic series allows to avoid a few calls to std::sqrt near 0,
     * which ADOL-C doesn't like.
     */
    static T sincHalfOfSquare(const T& x) {
        using std::sin;
        using std::sqrt;
        return (x < 1e-15) ? 0.5 - (x/48)  + (x*x)/(120*32) : sin(sqrt(x)/2)/sqrt(x);
    }

    /** \brief Computes sin(sqrt(x)) / sqrt(x) without getting unstable for small x
     *
     * Using this somewhat exotic series allows to avoid a few calls to std::sqrt near 0,
     * which ADOL-C doesn't like.
     */
    static T sincOfSquare(const T& x) {
        using std::sin;
        using std::sqrt;
        // we need here lots of terms to be sure that the numerical derivatives are also within maschine precision
        return (x < 1e-2) ?
        1-x/6
        +x*x/120
        -Dune::power(x,3)/5040
        +Dune::power(x,4)/362880
        -Dune::power(x,5)/39916800
        +Dune::power(x,6)/6227020800
        -Dune::power(x,7)/1307674368000: sin(sqrt(x))/sqrt(x);
    }

public:

    /** \brief The type used for coordinates */
    typedef T ctype;

    /** \brief The type used for global coordinates */
    typedef Dune::FieldVector<T,4> CoordinateType;

    /** \brief Dimension of the manifold formed by the 3d rotations */
    static const int dim = 3;

    /** \brief Coordinates are embedded into a four-dimension Euclidean space */
    static const int embeddedDim = 4;

    /** \brief Member of the corresponding Lie algebra.  This really is a skew-symmetric matrix */
    typedef Dune::FieldVector<T,3> TangentVector;

    /** \brief A tangent vector as a vector in the surrounding coordinate space */