text: // This file is part of Sophus.
  //
  // Copyright 2012-2013 Hauke Strasdat
  //
  // Permission is hereby granted, free of charge, to any person obtaining a copy
  // of this software and associated documentation files (the "Software"), to
  // deal in the Software without restriction, including without limitation the
  // rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
  // sell copies of the Software, and to permit persons to whom the Software is
  // furnished to do so, subject to the following conditions:
  //
  // The above copyright notice and this permission notice shall be included in
  // all copies or substantial portions of the Software.
  //
  // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  // IN THE SOFTWARE.
  
  #ifndef SOPHUS_SO2_HPP
  #define SOPHUS_SO2_HPP
  
  #include <complex>
  
  #include "sophus.hpp"
  
  ////////////////////////////////////////////////////////////////////////////
  // Forward Declarations / typedefs
  ////////////////////////////////////////////////////////////////////////////
  
  namespace Sophus {
  template<typename _Scalar, int _Options=0> class SO2Group;
  typedef SO2Group<double> SO2 EIGEN_DEPRECATED;
  typedef SO2Group<double> SO2d; /**< double precision SO2 */
  typedef SO2Group<float> SO2f;  /**< single precision SO2 */
  }
  
  ////////////////////////////////////////////////////////////////////////////
  // Eigen Traits (For querying derived types in CRTP hierarchy)
  ////////////////////////////////////////////////////////////////////////////
  
  ////////////////////////////////////////////////////////////////////////////
  // Eigen Traits (For querying derived types in CRTP hierarchy)
  ////////////////////////////////////////////////////////////////////////////
  
  namespace Eigen {
  namespace internal {
  
  template<typename _Scalar, int _Options>
  struct traits<Sophus::SO2Group<_Scalar,_Options> > {
    typedef _Scalar Scalar;
    typedef Matrix<Scalar,2,1> ComplexType;
  };
  
  template<typename _Scalar, int _Options>
  struct traits<Map<Sophus::SO2Group<_Scalar>, _Options> >
      : traits<Sophus::SO2Group<_Scalar, _Options> > {
    typedef _Scalar Scalar;
    typedef Map<Matrix<Scalar,2,1>,_Options> ComplexType;
  };
  
  template<typename _Scalar, int _Options>
  struct traits<Map<const Sophus::SO2Group<_Scalar>, _Options> >
      : traits<const Sophus::SO2Group<_Scalar, _Options> > {
    typedef _Scalar Scalar;
    typedef Map<const Matrix<Scalar,2,1>,_Options> ComplexType;
  };
  
  }
  }
  
  namespace Sophus {
  using namespace Eigen;
  
  /**
   * \brief SO2 base type - implements SO2 class but is storage agnostic
   *
   * [add more detailed description/tutorial]
   */
  template<typename Derived>
  class SO2GroupBase {
  public:
    /** \brief scalar type */
    typedef typename internal::traits<Derived>::Scalar Scalar;
    /** \brief complex number reference type */
    typedef typename internal::traits<Derived>::ComplexType &
    ComplexReference;
    /** \brief complex number const reference type */
    typedef const typename internal::traits<Derived>::ComplexType &
    ConstComplexReference;
  
    /** \brief degree of freedom of group
     *         (one for in-plane rotation) */
    static const int DoF = 1;
    /** \brief number of internal parameters used
     *         (unit complex number for rotation) */
    static const int num_parameters = 2;
    /** \brief group transformations are NxN matrices */
    static const int N = 2;
    /** \brief group transfomation type */
    typedef Matrix<Scalar,N,N> Transformation;
    /** \brief point type */
    typedef Matrix<Scalar,2,1> Point;
    /** \brief tangent vector type */
    typedef Scalar Tangent;
    /** \brief adjoint transformation type */
    typedef Scalar Adjoint;
  
    /**
     * \brief Adjoint transformation
     *
     * This function return the adjoint transformation \f$ Ad \f$ of the
     * group instance \f$ A \f$  such that for all \f$ x \f$
     * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$
     * with \f$\ \widehat{\cdot} \f$ being the hat()-operator.
     *
     * For SO2, it simply returns 1.
     */
    inline
    const Adjoint Adj() const {
      return 1;
    }
  
    /**
     * \returns copy of instance casted to NewScalarType
     */
    template<typename NewScalarType>
    inline SO2Group<NewScalarType> cast() const {
      return SO2Group<NewScalarType>(unit_complex()
                                     .template cast<NewScalarType>() );
    }
  
    /**
     * \returns pointer to internal data
     *
     * This provides unsafe read/write access to internal data. SO2 is represented
     * by a complex number with unit length (two parameters). When using direct
     * write access, the user needs to take care of that the complex number stays
     * normalized.
     *
     * \see normalize()
     */
    inline Scalar* data() {
      return unit_complex_nonconst().data();
    }
  
    /**
     * \returns const pointer to internal data
     *
     * Const version of data().
     */
    inline const Scalar* data() const {
      return unit_complex().data();
    }
  
    /**
     * \brief Fast group multiplication
     *
     * This method is a fast version of operator*=(), since it does not perform
     * normalization. It is up to the user to call normalize() once in a while.
     *
     * \see operator*=()
     */
    inline
    void fastMultiply(const SO2Group<Scalar>& other) {
      Scalar lhs_real = unit_complex().x();
      Scalar lhs_imag = unit_complex().y();
      const Scalar & rhs_real = other.unit_complex().x();
      const Scalar & rhs_imag = other.unit_complex().y();
      // complex multiplication
      unit_complex_nonconst().x() = lhs_real*rhs_real - lhs_imag*rhs_imag;
      unit_complex_nonconst().y() = lhs_real*rhs_imag + lhs_imag*rhs_real;
    }
  
    /**
     * \returns group inverse of instance
     */
    inline
    const SO2Group<Scalar> inverse() const {
      return SO2Group<Scalar>(unit_complex().x(), -unit_complex().y());
    }
  
    /**
     * \brief Logarithmic map
     *
     * \returns tangent space representation (=rotation angle) of instance
     *
     * \see  log().
     */
    inline
    const Scalar log() const {
      return SO2Group<Scalar>::log(*this);
    }
  
    /**
     * \brief Normalize complex number
     *
     * It re-normalizes complex number to unit length. This method only needs to
     * be called in conjunction with fastMultiply() or data() write access.
     */
    inline
    void normalize() {
      Scalar length =
          std::sqrt(unit_complex().x()*unit_complex().x()
               + unit_complex().y()*unit_complex().y());
      if(length < SophusConstants<Scalar>::epsilon()) {
        throw SophusException("Complex number is (near) zero!");
      }
      unit_complex_nonconst().x() /= length;
      unit_complex_nonconst().y() /= length;
    }
  
    /**
     * \returns 2x2 matrix representation of instance
     *
     * For SO2, the matrix representation is an orthogonal matrix R with det(R)=1,
     * thus the so-called rotation matrix.
     */
    inline
    const Transformation matrix() const {
      const Scalar & real = unit_complex().x();
      const Scalar & imag = unit_complex().y();
      Transformation R;
      R << real, -imag
          ,imag,  real;
      return R;
    }
  
    /**
     * \brief Assignment operator
     */
    template<typename OtherDerived> inline
    SO2GroupBase<Derived>& operator=(const SO2GroupBase<OtherDerived> & other) {
      unit_complex_nonconst() = other.unit_complex();
      return *this;
    }
  
    /**
     * \brief Group multiplication
     * \see operator*=()
     */
    inline
    const SO2Group<Scalar> operator*(const SO2Group<Scalar>& other) const {
      SO2Group<Scalar> result(*this);
      result *= other;
      return result;
    }
  
    /**
     * \brief Group action on \f$ \mathbf{R}^2 \f$
     *
     * \param p point \f$p \in \mathbf{R}^2 \f$
     * \returns point \f$p' \in \mathbf{R}^2 \f$, rotated version of \f$p\f$
     *
     * This function rotates a point \f$ p \f$ in  \f$ \mathbf{R}^2 \f$ by the
     * SO2 transformation \f$R\f$ (=rotation matrix): \f$ p' = R\cdot p \f$.
     */
    inline
    const Point operator*(const Point & p) const {
      const Scalar & real = unit_complex().x();
      const Scalar & imag = unit_complex().y();
      return Point(real*p[0] - imag*p[1], imag*p[0] + real*p[1]);
    }
  
    /**
     * \brief In-place group multiplication
     *
     * \see fastMultiply()
     * \see operator*()
     */
    inline
    void operator*=(const SO2Group<Scalar>& other) {
      fastMultiply(other);
      normalize();
    }
  
    /**
     * \brief Setter of internal unit complex number representation
     *
     * \param complex
     * \pre   the complex number must not be near zero
     *
     * The complex number is normalized to unit length.
     */
    inline
    void setComplex(const Point & complex) {
      unit_complex_nonconst() = complex;
      normalize();
    }
  
    /**
     * \brief Accessor of unit complex number
     *
     * No direct write access is given to ensure the complex stays normalized.
     */
    EIGEN_STRONG_INLINE
    ConstComplexReference unit_complex() const {
      return static_cast<const Derived*>(this)->unit_complex();
    }
  
    ////////////////////////////////////////////////////////////////////////////
    // public static functions
    ////////////////////////////////////////////////////////////////////////////
  
    /**
     * \brief Group exponential
     *
     * \param theta tangent space element (=rotation angle \f$ \theta \f$)
     * \returns     corresponding element of the group SO2
     *
     * To be more specific, this function computes \f$ \exp(\widehat{\theta}) \f$
     * with \f$ \exp(\cdot) \f$ being the matrix exponential
     * and \f$ \widehat{\cdot} \f$ the hat()-operator of SO2.
     *
     * \see hat()
     * \see log()
     */
    inline static
    const SO2Group<Scalar> exp(const Tangent & theta) {
      return SO2Group<Scalar>(std::cos(theta), std::sin(theta));
    }
  
    /**
     * \brief Generator
     *
     * The infinitesimal generator of SO2
     * is \f$
     *        G_0 = \left( \begin{array}{ccc}
     *                          0& -1& \\
     *                          1&  0&
     *                     \end{array} \right).
     * \f$
     * \see hat()
     */
    inline static
    const Transformation generator() {
      return hat(1);
    }
  
    /**
     * \brief hat-operator
     *
     * \param theta scalar representation of Lie algebra element
     * \returns     2x2-matrix representatin of Lie algebra element
     *
     * Formally, the hat-operator of SO2 is defined
     * as \f$ \widehat{\cdot}: \mathbf{R}^2 \rightarrow \mathbf{R}^{2\times 2},
     * \quad \widehat{\theta} = G_0\cdot \theta \f$
     * with \f$ G_0 \f$ being the infinitesial generator().
     *
     * \see generator()
     * \see vee()
     */
    inline static
    const Transformation hat(const Tangent & theta) {
      Transformation Omega;
      Omega <<  static_cast<Scalar>(0), -theta
          ,  theta,     static_cast<Scalar>(0);
      return Omega;
    }
  
    /**
     * \brief Lie bracket
     *
     * \param theta1 scalar representation of Lie algebra element
     * \param theta2 scalar representation of Lie algebra element
     * \returns      zero
     *
     * It computes the bracket. For the Lie algebra so2, the Lie bracket is
     * simply \f$ [\theta_1, \theta_2]_{so2} = 0 \f$ since SO2 is a
     * commutative group.
     *
     * \see hat()
     * \see vee()
     */
    inline static
    const Tangent lieBracket(const Tangent & theta1,
                             const Tangent & theta2) {
      return static_cast<Scalar>(0);
    }
  
    /**
     * \brief Logarithmic map
     *
     * \param other element of the group SO2
     * \returns     corresponding tangent space element
     *              (=rotation angle \f$ \theta \f$)
     *
     * Computes the logarithmic, the inverse of the group exponential.
     * To be specific, this function computes \f$ \log({\cdot})^\vee \f$
     * with \f$ \vee(\cdot) \f$ being the matrix logarithm
     * and \f$ \vee{\cdot} \f$ the vee()-operator of SO2.
     *
     * \see exp()
     * \see vee()
     */
    inline static
    const Tangent log(const SO2Group<Scalar> & other) {
      // todo: general implementation for Scalar not being float or double.
      return atan2(other.unit_complex_.y(), other.unit_complex().x());
    }
  
    /**
     * \brief vee-operator
     *
     * \param Omega 2x2-matrix representation of Lie algebra element
     * \pre         Omega need to be a skew-symmetric matrix
     * \returns     scalar representatin of Lie algebra element
     *s
     * This is the inverse of the hat()-operator.
     *
     * \see hat()
     */
    inline static
    const Tangent vee(const Transformation & Omega) {
      return static_cast<Scalar>(0.5)*(Omega(1,0) - Omega(0,1));
    }
  
  private:
    // Mutator of complex number is private so users are hampered
    // from setting non-unit complex numbers.
    EIGEN_STRONG_INLINE
    ComplexReference unit_complex_nonconst() {
      return static_cast<Derived*>(this)->unit_complex_nonconst();
    }
  
  };
  
  /**
   * \brief SO2 default type - Constructors and default storage for SO2 Type
   */
  template<typename _Scalar, int _Options>
  class SO2Group : public SO2GroupBase<SO2Group<_Scalar,_Options> > {
    typedef SO2GroupBase<SO2Group<_Scalar,_Options> > Base;
  public:
    /** \brief scalar type */
    typedef typename internal::traits<SO2Group<_Scalar,_Options> >
    ::Scalar Scalar;
    /** \brief complex number reference type */
    typedef typename internal::traits<SO2Group<_Scalar,_Options> >
    ::ComplexType & ComplexReference;
    /** \brief complex number const reference type */
    typedef const typename internal::traits<SO2Group<_Scalar,_Options> >
    ::ComplexType & ConstComplexReference;
  
    /** \brief degree of freedom of group */
    static const int DoF = Base::DoF;
    /** \brief number of internal parameters used */
    static const int num_parameters = Base::num_parameters;
    /** \brief group transformations are NxN matrices */
    static const int N = Base::N;
    /** \brief group transfomation type */
    typedef typename Base::Transformation Transformation;
    /** \brief point type */
    typedef typename Base::Point Point;
    /** \brief tangent vector type */
    typedef typename Base::Tangent Tangent;
    /** \brief adjoint transformation type */
    typedef typename Base::Adjoint Adjoint;
  
    // base is friend so unit_complex_nonconst can be accessed from base
    friend class SO2GroupBase<SO2Group<_Scalar,_Options> >;
  
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  
    /**
     * \brief Default constructor
     *
     * Initialize complex number to identity rotation.
     */
    inline SO2Group()
      : unit_complex_(static_cast<Scalar>(1), static_cast<Scalar>(0)) {
    }
  
    /**
     * \brief Copy constructor
     */
    template<typename OtherDerived> inline
    SO2Group(const SO2GroupBase<OtherDerived> & other)
      : unit_complex_(other.unit_complex()) {
    }
  
    /**
     * \brief Constructor from rotation matrix
     *
     * \pre rotation matrix need to be orthogonal with determinant of 1
     */
    inline explicit
    SO2Group(const Transformation & R)
      : unit_complex_(static_cast<Scalar>(0.5)*(R(0,0)+R(1,1)),
                      static_cast<Scalar>(0.5)*(R(1,0)-R(0,1))) {
      if (std::abs(R.determinant()-static_cast<Scalar>(1))
          > SophusConstants<Scalar>::epsilon()) {
        throw SophusException("det(R) is not near 1.");
      }
    }
  
    /**
     * \brief Constructor from pair of real and imaginary number
     *
     * \pre pair must not be zero
     */
    inline SO2Group(const Scalar & real, const Scalar & imag)
      : unit_complex_(real, imag) {
      Base::normalize();
    }
  
    /**
     * \brief Constructor from 2-vector
     *
     * \pre vector must not be zero
     */
    inline explicit
    SO2Group(const Matrix<Scalar,2,1> & complex)
      : unit_complex_(complex) {
      Base::normalize();
    }
  
    /**
     * \brief Constructor from std::complex
     *
     * \pre complex number must not be zero
     */
    inline explicit
    SO2Group(const std::complex<Scalar> & complex)
      : unit_complex_(complex.real(), complex.imag()) {
      Base::normalize();
    }
  
    /**
     * \brief Constructor from an angle
     */
    inline explicit
    SO2Group(Scalar theta) {
      unit_complex_nonconst() = SO2Group<Scalar>::exp(theta).unit_complex();
    }
  
    /**
     * \brief Accessor of unit complex number
     *
     * No direct write access is given to ensure the complex number stays
     * normalized.
     */
    EIGEN_STRONG_INLINE
    ConstComplexReference unit_complex() const {
      return unit_complex_;
    }
  
  protected:
    // Mutator of complex number is protected so users are hampered
    // from setting non-unit complex numbers.
    EIGEN_STRONG_INLINE
    ComplexReference unit_complex_nonconst() {
      return unit_complex_;
    }
  
    static bool isNearZero(const Scalar & real, const Scalar & imag) {
      return (real*real + imag*imag < SophusConstants<Scalar>::epsilon());
    }
  
    Matrix<Scalar,2,1> unit_complex_;
  };
  
  } // end namespace
  
  
  namespace Eigen {
  /**
   * \brief Specialisation of Eigen::Map for SO2GroupBase
   *
   * Allows us to wrap SO2 Objects around POD array
   * (e.g. external c style complex number)
   */
  template<typename _Scalar, int _Options>
  class Map<Sophus::SO2Group<_Scalar>, _Options>
      : public Sophus::SO2GroupBase<Map<Sophus::SO2Group<_Scalar>, _Options> > {
    typedef Sophus::SO2GroupBase<Map<Sophus::SO2Group<_Scalar>, _Options> > Base;
  
  public:
    /** \brief scalar type */
    typedef typename internal::traits<Map>::Scalar Scalar;
    /** \brief complex number reference type */
    typedef typename internal::traits<Map>::ComplexType & ComplexReference;
    /** \brief complex number const reference type */
    typedef const typename internal::traits<Map>::ComplexType &
    ConstComplexReference;
  
    /** \brief degree of freedom of group */
    static const int DoF = Base::DoF;
    /** \brief number of internal parameters used */
    static const int num_parameters = Base::num_parameters;
    /** \brief group transformations are NxN matrices */
    static const int N = Base::N;
    /** \brief group transfomation type */
    typedef typename Base::Transformation Transformation;
    /** \brief point type */
    typedef typename Base::Point Point;
    /** \brief tangent vector type */
    typedef typename Base::Tangent Tangent;
    /** \brief adjoint transformation type */
    typedef typename Base::Adjoint Adjoint;
  
    // base is friend so unit_complex_nonconst can be accessed from base
    friend class Sophus::SO2GroupBase<Map<Sophus::SO2Group<_Scalar>, _Options> >;
  
    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
    using Base::operator*=;
    using Base::operator*;
  
    EIGEN_STRONG_INLINE
    Map(Scalar* coeffs) : unit_complex_(coeffs) {
    }
  
    /**
     * \brief Accessor of unit complex number
     *
     * No direct write access is given to ensure the complex number stays
     * normalized.
     */
    EIGEN_STRONG_INLINE
    ConstComplexReference unit_complex() const {
      return unit_complex_;
    }
  
  protected:
    // Mutator of complex number is protected so users are hampered
    // from setting non-unit complex number.
    EIGEN_STRONG_INLINE
    ComplexReference unit_complex_nonconst() {
      return unit_complex_;
    }
  
    Map<Matrix<Scalar,2,1>,_Options> unit_complex_;
  };
  
  /**
   * \brief Specialisation of Eigen::Map for const SO2GroupBase
   *
   * Allows us to wrap SO2 Objects around POD array
   * (e.g. external c style complex number)
   */
  template<typename _Scalar, int _Options>
  class Map<const Sophus::SO2Group<_Scalar>, _Options>
      : public Sophus::SO2GroupBase<
      Map<const Sophus::SO2Group<_Scalar>, _Options> > {
    typedef Sophus::SO2GroupBase<Map<const Sophus::SO2Group<_Scalar>, _Options> >
    Base;
  
  public:
    /** \brief scalar type */
    typedef typename internal::traits<Map>::Scalar Scalar;
    /** \brief complex number const reference type */
    typedef const typename internal::traits<Map>::ComplexType &
    ConstComplexReference;
  
  
    /** \brief degree of freedom of group */
    static const int DoF = Base::DoF;
    /** \brief number of internal parameters used */
    static const int num_parameters = Base::num_parameters;
    /** \brief group transformations are NxN matrices */
    static const int N = Base::N;
    /** \brief group transfomation type */
    typedef typename Base::Transformation Transformation;
    /** \brief point type */
    typedef typename Base::Point Point;
    /** \brief tangent vector type */
    typedef typename Base::Tangent Tangent;
    /** \brief adjoint transformation type */
    typedef typename Base::Adjoint Adjoint;
  
    EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
    using Base::operator*=;
    using Base::operator*;
  
    EIGEN_STRONG_INLINE
    Map(const Scalar* coeffs) : unit_complex_(coeffs) {
    }
  
    /**
     * \brief Accessor of unit complex number
     *
     * No direct write access is given to ensure the complex number stays
     * normalized.
     */
    EIGEN_STRONG_INLINE
    ConstComplexReference unit_complex() const {
      return unit_complex_;
    }
  
  protected:
    const Map<const Matrix<Scalar,2,1>,_Options> unit_complex_;
  };
  
  }
  
  
  #endif // SOPHUS_SO2_HPP
  