angle.hpp

Go to the documentation of this file.

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef ECL_GEOMETRY_ANGLE_HPP_
#define ECL_GEOMETRY_ANGLE_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

#include <cmath>
#include <ecl/config/macros.hpp>
#include <ecl/linear_algebra.hpp>
#include <ecl/type_traits/fundamental_types.hpp>
#include <ecl/mpl/enable_if.hpp>
#include <ecl/math/constants.hpp>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace ecl {

template <typename T>
ECL_PUBLIC T radians_to_degrees(const T &radians, typename enable_if<ecl::is_float<T> >::type* dummy = 0) {
        static const T rads_to_degs = 180.0/pi;
        return radians*rads_to_degs;
}

template <typename T>
ECL_PUBLIC T degrees_to_radians(const T &degrees, typename enable_if<ecl::is_float<T> >::type* dummy = 0) {
        static const T degs_to_rads = pi/180.0;
        return degrees*degs_to_rads;
}

ECL_PUBLIC const float& wrap_angle(float &angle);

ECL_PUBLIC float wrap_angle(const float &angle);
ECL_PUBLIC const double& wrap_angle(double &angle);

ECL_PUBLIC double wrap_angle(const double &angle);

/*****************************************************************************
** Interface [Angle]
*****************************************************************************/
template <class T, typename Enable = void>
class ECL_LOCAL Angle {
private:
        Angle() {};
};

template <typename T>
class ECL_PUBLIC Angle<T, typename enable_if<is_float<T> >::type> {
public:
        /*********************
        ** Typedefs
        **********************/
        typedef ecl::linear_algebra::Matrix<T,2,2> RotationMatrix;
        Angle(const T &angle = 0.0) : value(angle) { wrap_angle(value); }
        Angle(const RotationMatrix &rotation) {
                value = std::atan2(rotation(1,0),rotation(0,0));
        }

        const Angle<T>& operator=(const T &angle);
        /*********************
        ** Math Operators
        **********************/
        Angle<T> operator+(const T &angle) const;
        Angle<T> operator+(const Angle<T> &angle) const;
        void operator+=(const T &angle);
        void operator+=(const Angle<T> &angle);
        Angle<T> operator-(const T &angle) const;
        Angle<T> operator-(const Angle<T> &angle) const;
        void operator-=(const T &angle);
        void operator-=(const Angle<T> &angle);
        Angle<T> operator*(const T &scalar) const;
        void operator*=(const T &scalar);

        /*********************
        ** Casting
        **********************/
        operator const T&() const { return value; }
        T degrees();

        RotationMatrix rotationMatrix() const {
                T c = std::cos(value);
                T s = std::sin(value);
                return (RotationMatrix() << c, -s, s, c).finished();
        }

        static Angle<T> Degrees(const T &angle);
        static Angle<T> Radians(const T &angle);

private:
        T value;
};

/*****************************************************************************
** Implementation
*****************************************************************************/

template <typename T>
T Angle<T, typename enable_if<is_float<T> >::type>::degrees() {
        return radians_to_degrees(value);
}

template <typename T>
const Angle<T>& Angle<T, typename enable_if<is_float<T> >::type>::operator=(const T &angle) {
        value = angle;
        wrap_angle(value);
        return *this;
}

/*****************************************************************************
** Operators
*****************************************************************************/

template <typename T>
Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator+(const T &angle) const {
        return Angle<T>(wrap_angle(value+angle));
}

template <typename T>
Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator+(const Angle<T> &angle) const {
        return Angle<T>(wrap_angle(value+angle.value));
}

template <typename T>
void Angle<T, typename enable_if<is_float<T> >::type>::operator+=(const T &angle) {
        value += angle;
        wrap_angle(value);
}

template <typename T>
void Angle<T, typename enable_if<is_float<T> >::type>::operator+=(const Angle<T> &angle) {
        value += angle.value;
        wrap_angle(value);
}

template <typename T>
Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator-(const T &angle) const {
        return Angle<T>(wrap_angle(value-angle));
}

template <typename T>
Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator-(const Angle<T> &angle) const {
        return Angle<T>(wrap_angle(value-angle.value));
}

template <typename T>
void Angle<T, typename enable_if<is_float<T> >::type>::operator-=(const T &angle) {
        value -= angle;
        wrap_angle(value);
}

template <typename T>
void Angle<T, typename enable_if<is_float<T> >::type>::operator-=(const Angle<T> &angle) {
        value -= angle.value;
        wrap_angle(value);
}

template <typename T>
Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator*(const T &scalar) const {
        return Angle<T>(wrap_angle(scalar*value));
}

template <typename T>
void Angle<T, typename enable_if<is_float<T> >::type>::operator*=(const T &scalar) {
        value *= scalar;
        wrap_angle(value);
}

/*****************************************************************************
** Statics
*****************************************************************************/

template <typename T>
Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::Degrees(const T &angle) {
        return Angle<T>(degrees_to_radians(angle));
}

template <typename T>
Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::Radians(const T &angle) {
        return Angle<T>(angle);
}
} // namespace ecl

#endif /* ECL_GEOMETRY_ANGLE_HPP_ */