Angle.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#include <type_traits>

#include <error/Exceptions.h>

#include <platform/Platform.h> // for MIRA_DEPRECATED
#include <math/Math.h>

#include <serialization/Accessor.h>


#ifndef _MIRA_ANGLE_H_
#define _MIRA_ANGLE_H_

namespace mira {

namespace Private {
template <typename Param, typename Result>
inline Result deg2rad(Param value) {
    return static_cast<Result>(value * pi_div_deg180<Result>());
}
template <typename Param, typename Result>
inline Result rad2deg(Param value) {
    return static_cast<Result>(value * deg180_div_pi<Result>());
}
}

template <typename T>
inline
typename std::enable_if<std::is_floating_point<T>::value, T>::type
deg2rad(T value) {
    return Private::deg2rad<T, T>(value);
}

template <typename T>
inline
typename std::enable_if<std::is_integral<T>::value, double>::type
deg2rad(T value) {
    return Private::deg2rad<T, double>(value);
}

// disable for non-arithmetic types, with clear compile-time error message
template <typename T>
typename std::enable_if<!std::is_arithmetic<T>::value>::type // = void if enabled
deg2rad(T value) {
    static_assert(sizeof(T)==0,
                  "deg2rad must be used with arithmetic types");
}

template <typename T>
inline
typename std::enable_if<std::is_floating_point<T>::value, T>::type
rad2deg(T value) {
    return Private::rad2deg<T, T>(value);
}

template <typename T>
inline
typename std::enable_if<std::is_integral<T>::value, double>::type
rad2deg(T value) {
    return Private::rad2deg<T, double>(value);
}

// disable for non-arithmetic types, with clear compile-time error message
template <typename T>
typename std::enable_if<!std::is_arithmetic<T>::value>::type // = void if enabled
rad2deg(T value) {
    static_assert(sizeof(T)==0,
                  "rad2deg must be used with arithmetic types");
}


namespace Private {
struct DegreeUnitTag {
    static const char* unit() { return "deg"; }
};
struct RadianUnitTag {
    static const char* unit() { return "rad"; }
};

// helper for converting angles from FromTag to ToTag, e.g. DegreeUnitTag to RadianUnitTag.
template <typename T, typename FromTag, typename ToTag>
struct AngleConverter {
    static_assert(sizeof(FromTag)==0, "Angle conversion not specialized!");
    static T convert(T value) { return static_cast<T>(0); }
};


// specializations for the actual conversions
template <typename T>
struct AngleConverter<T,DegreeUnitTag,RadianUnitTag> {
    static T convert(T value) { return mira::deg2rad(value); }
};

template <typename T>
struct AngleConverter<T,RadianUnitTag,DegreeUnitTag> {
    static T convert(T value) { return mira::rad2deg(value); }
};

template <typename T,typename SameTag>
struct AngleConverter<T,SameTag,SameTag> {
    static T convert(T value) { return value; }
};

template <typename A, typename B>
struct AnglePromoteHelper {
    typedef decltype(A(0) + B(0)) type;
};

}

template <typename T, typename UnitTag, typename Derived>
class AngleBase
{
protected:

    // direct fast initialization without normalization
    AngleBase(T value, int) : mValue(value) {}

    AngleBase() {}
    explicit AngleBase(T value)  { setValue(value); }
    AngleBase(const AngleBase& other) : mValue(other.mValue) {}

    template <typename OtherT, typename OtherUnitTag, typename OtherDerived>
    explicit AngleBase(const AngleBase<OtherT,OtherUnitTag,OtherDerived>& other) {
        setValue(conv(other));
    }

    // generated copy constructor is suitable

    // assignment operator implementations that are used by derived classes
    template <typename OtherUnitTag, typename OtherDerived>
    void operator=(const AngleBase<T,OtherUnitTag,OtherDerived>& other) {
        mValue = conv(other);
    }

    void operator=(const T& other) {
        setValue(other);
    }

public:

    template <typename Reflector>
    void reflect(Reflector& r) {
        r.delegate(mValue);
    }

public:

    static T upper() { return Derived::lower() + Derived::turn(); }


public:

    void setValue(const T& value)
    {
        mValue = value;
        if(mValue < Derived::upper() && mValue >= Derived::lower())
            return;
        // for deviations of two times turn from the allowed interval the floor
        // operation is faster than a while loop
        // therefore, the explicit check for interval in the line above makes sense
        // to make it faster if no clipping is necessary
#ifdef MIRA_LINUX
        mValue -= Derived::turn() * floor((mValue - Derived::lower())/ Derived::turn());
#endif
#ifdef MIRA_WINDOWS
        mValue -= Derived::turn() * floor((double)(mValue - Derived::lower())/ Derived::turn());
#endif
        //for integer types the floor operation fails
        if(mValue < Derived::lower())
            mValue += Derived::turn();
    }

    const T& value() const { return mValue; }

    explicit operator T() const { return mValue; }

    void setSerializedValue(const T& value) {
        setValue(Derived::convertFromSerialized(value));
    }

    T serializedValue() const { return Derived::convertToSerialized(mValue); }


public:

    T deg() const { return This()->toDeg(); }

    T rad() const { return This()->toRad();}

public:
    // operators

    // note the generic operators take Derived& as first argument, while
    // the operators, where T& is a parameter take AngleBase& as argument.
    // this is necessary to resolve the ISO C++ ambiguity with type conversation

    template <typename OtherUnitTag, typename OtherDerived>
    friend Derived operator+(const Derived& a, const AngleBase<T,OtherUnitTag,OtherDerived>&  b) {
        return Derived( a.mValue + conv(b) );
    }
    friend Derived operator+(const AngleBase& a, const T& b) {
        return Derived( a.mValue + b);
    }
    friend Derived operator+(const T& a, const AngleBase& b) {
        return Derived( a + b.mValue);
    }

    template <typename OtherUnitTag, typename OtherDerived>
    friend Derived operator-(const Derived& a,const AngleBase<T,OtherUnitTag,OtherDerived>&  b) {
        return Derived( a.mValue - conv(b) );
    }
    friend Derived operator-(const AngleBase& a, const T& b) {
        return Derived( a.mValue - b);
    }
    friend Derived operator-(const T& a, const AngleBase& b) {
        return Derived( a - b.mValue);
    }

    Derived operator-() const {
        return Derived(-mValue);
    }

    friend Derived operator*(const Derived& a, const T& b) {
        return Derived( a.mValue * b);
    }
    friend Derived operator*(const T& a, const Derived& b) {
        return Derived( a * b.mValue);
    }

    friend Derived operator/(const Derived& a, const T& b) {
        return Derived( a.mValue / b);
    }


    template <typename OtherUnitTag, typename OtherDerived>
    Derived operator+=(const AngleBase<T,OtherUnitTag,OtherDerived>& a) {
        setValue(mValue+conv(a));
        return *This();
    }
    Derived& operator+=(const T& a) {
        setValue(mValue+a);
        return *This();
    }

    template <typename OtherUnitTag, typename OtherDerived>
    Derived operator-=(const AngleBase<T,OtherUnitTag,OtherDerived>& a) {
        setValue(mValue-conv(a));
        return *This();
    }
    Derived& operator-=(const T& a) {
        setValue(mValue-a);
        return *This();
    }

    Derived& operator*=(const T& s) {
        setValue(mValue*s);
        return *This();
    }

    Derived& operator/=(const T& s) {
        setValue(mValue/s);
        return *This();
    }


    friend bool operator<(const AngleBase& a, const AngleBase& b) { return a.mValue < b.mValue; }
    friend bool operator<=(const AngleBase& a, const AngleBase& b) { return a.mValue <= b.mValue; }
    friend bool operator>(const AngleBase& a, const AngleBase& b) { return a.mValue > b.mValue; }
    friend bool operator>=(const AngleBase& a, const AngleBase& b) { return a.mValue >= b.mValue; }
    friend bool operator==(const AngleBase& a, const AngleBase& b) { return a.mValue == b.mValue; }
    friend bool operator!=(const AngleBase& a, const AngleBase& b) { return a.mValue != b.mValue; }


public:

    T smallestDifferenceValue(const Derived& other) const
    {
        T d = mValue - other.mValue;

        static const T halfTurn = Derived::turn() / 2;
        if(d > halfTurn)
            d -= Derived::turn();
        else if(d <= -halfTurn)
            d += Derived::turn();

        return d;
    }

    bool isInInterval(const Derived& min, const Derived& max) const
    {
        if (min.mValue <= max.mValue)
            return mValue >= min.mValue && mValue <= max.mValue;
        return mValue >= min.mValue || mValue <= max.mValue;
    }

public:

    friend std::ostream& operator<<(std::ostream& o, const AngleBase& v ) {
        o << v.value() << " " << UnitTag::unit();
        return o;
    }

public:

    static const char* unit() {
        return UnitTag::unit();
    }

private:

    template <typename OtherT, typename OtherUnitTag, typename OtherDerived>
    static T conv(const AngleBase<OtherT,OtherUnitTag,OtherDerived>& other) {
        typedef typename Private::AnglePromoteHelper<T,OtherT>::type type;
        return Private::AngleConverter<type,OtherUnitTag,UnitTag>::convert(other.value());
    }


    Derived* This() { return static_cast<Derived*>(this); }

    const Derived* This() const { return static_cast<const Derived*>(this); }

protected:

    T mValue;
};


#define MIRA_ANGLE_CONSTRUCTORS_AND_ASSIGNOPS(Type)                            \
protected:                                                                     \
    Type(const T& value, int) : Base(value,1) {}                               \
public:                                                                        \
    Type() {}                                                                  \
    explicit Type(T value) : Base(value) {}                                    \
    template <typename OtherT, typename OtherUnitTag, typename OtherDerived>   \
    Type(const AngleBase<OtherT,OtherUnitTag,OtherDerived>& other) : Base(other) {} \
    template <typename OtherUnitTag, typename OtherDerived>                    \
    Type& operator=(const AngleBase<T,OtherUnitTag,OtherDerived>& other) {     \
        Base::operator=(other);                                                \
        return *this; }                                                        \
    Type& operator=(const T& other) {                                          \
        Base::operator=(other);                                                \
        return *this; }



template <typename T, typename Derived>
class DegreeBase : public AngleBase<T, Private::DegreeUnitTag, Derived>
{
    friend class AngleBase<T, Private::DegreeUnitTag, Derived>;
    typedef AngleBase<T, Private::DegreeUnitTag, Derived> Base;
public:
    MIRA_ANGLE_CONSTRUCTORS_AND_ASSIGNOPS(DegreeBase)

public:
    static T turn() { return (T)360; }
    
    static T convertToSerialized(T value) { return value; }

    static T convertFromSerialized(T value) { return value; }

private:
    T toDeg() const { return this->mValue; }

    T toRad() const { return deg2rad(this->mValue); }
};

template <typename T, typename Derived>
class RadianBase : public AngleBase<T, Private::RadianUnitTag, Derived>
{
    static_assert(std::is_floating_point<T>::value,
            "Radians make sense with floating point types only. "
            "Use Degree if you want to represent angles with integral types");

    friend class AngleBase<T, Private::RadianUnitTag, Derived>;
    typedef AngleBase<T, Private::RadianUnitTag, Derived> Base;

public:
    MIRA_ANGLE_CONSTRUCTORS_AND_ASSIGNOPS(RadianBase)

public:
    static T turn() { return two_pi<T>(); }

    static T convertToSerialized(T value) { return value; }

    static T convertFromSerialized(T value) { return value; }

private:
    T toDeg() const { return rad2deg(this->mValue); }

    T toRad() const { return this->mValue; }

};


// forward decl
template <typename T>
class Degree;

template <typename T>
class SignedDegree : public DegreeBase<T, SignedDegree<T>>
{
    typedef DegreeBase<T, SignedDegree<T>> Base;
    friend class Degree<T>;
public:
    MIRA_ANGLE_CONSTRUCTORS_AND_ASSIGNOPS(SignedDegree)

    
    static T lower() { return static_cast<T>(-180); }

    SignedDegree<T> smallestDifference(const SignedDegree& other) const {
        return SignedDegree<T>(this->smallestDifferenceValue(other), 1);
    }
};

template <typename T, typename SerializerTag>
class IsTransparentSerializable<SignedDegree<T>,SerializerTag> : public std::true_type {};


template <typename T>
class Degree : public DegreeBase<T, Degree<T>>
{
    typedef DegreeBase<T, Degree<T>> Base;
public:
    MIRA_ANGLE_CONSTRUCTORS_AND_ASSIGNOPS(Degree)

    
    static T lower() { return static_cast<T>(0); }

    SignedDegree<T> smallestDifference(const Degree& other) const {
        return SignedDegree<T>(this->smallestDifferenceValue(other), 1);
    }
};

template <typename T, typename SerializerTag>
class IsTransparentSerializable<Degree<T>,SerializerTag> : public std::true_type {};


// forward decl
template <typename T>
class Radian;

template <typename T>
class SignedRadian : public RadianBase<T, SignedRadian<T>>
{
    typedef RadianBase<T, SignedRadian<T>> Base;
    friend class Radian<T>;
public:
    MIRA_ANGLE_CONSTRUCTORS_AND_ASSIGNOPS(SignedRadian)

    
    static T lower() { return -pi<T>(); }

    SignedRadian<T> smallestDifference(const SignedRadian& other) const {
        return SignedRadian<T>(this->smallestDifferenceValue(other), 1);
    }
};

template <typename T, typename SerializerTag>
class IsTransparentSerializable<SignedRadian<T>,SerializerTag> : public std::true_type {};

template <typename T>
class Radian : public RadianBase<T, Radian<T>>
{
    typedef RadianBase<T, Radian<T>> Base;
public:
    MIRA_ANGLE_CONSTRUCTORS_AND_ASSIGNOPS(Radian)

    
    static T lower() { return static_cast<T>(0); }

    SignedRadian<T> smallestDifference(const Radian& other) const {
        return SignedRadian<T>(this->smallestDifferenceValue(other), 1);
    }
};

template <typename T, typename SerializerTag>
class IsTransparentSerializable<Radian<T>,SerializerTag> : public std::true_type {};


// forward decl
template <typename T>
class Angle;

template <typename T>
class SignedAngle : public RadianBase<T, SignedAngle<T>>
{
    typedef RadianBase<T, SignedAngle<T>> Base;
    friend class Angle<T>;
public:
    MIRA_ANGLE_CONSTRUCTORS_AND_ASSIGNOPS(SignedAngle)

    template <typename Reflector>
    void reflect(Reflector& r) {
        r.delegate(getter<T>(convertToSerialized, this->mValue),
                   setter<T>(convertFromSerialized, this->mValue));
    }

    static T lower() { return -pi<T>(); }

    SignedAngle<T> smallestDifference(const SignedAngle& other) const {
        return SignedAngle<T>(this->smallestDifferenceValue(other), 1);
    }


    MIRA_DEPRECATED("use Radian<T>(value)", static SignedAngle fromRad(T value)) {
        return SignedAngle(Radian<T>(value));
    }
    MIRA_DEPRECATED("use Degree<T>(value)", static SignedAngle fromDeg(T value)) {
        return SignedAngle(Degree<T>(value));
    }

public:
    // overwite convertXYZSerialized since Angle uses different internal and serialized representation.
    static T convertToSerialized(T value) { return rad2deg<T>(value); }

    static T convertFromSerialized(T value) { return deg2rad<T>(value); }
};

template <typename T, typename SerializerTag>
class IsTransparentSerializable<SignedAngle<T>,SerializerTag> : public std::true_type {};


template <typename T>
class Angle : public RadianBase<T, Angle<T>>
{
    typedef RadianBase<T, Angle<T>> Base;
public:
    MIRA_ANGLE_CONSTRUCTORS_AND_ASSIGNOPS(Angle)

    template <typename Reflector>
    void reflect(Reflector& r) {
        r.delegate(getter<T>(convertToSerialized, this->mValue),
                   setter<T>(convertFromSerialized, this->mValue));
    }

    static T lower() { return static_cast<T>(0); }

    SignedAngle<T> smallestDifference(const Angle& other) const {
        return SignedAngle<T>(this->smallestDifferenceValue(other), 1);
    }

    MIRA_DEPRECATED("use Radian<T>(value)", static Angle fromRad(T value)) {
        return Angle(Radian<T>(value));
    }
    MIRA_DEPRECATED("use Degree<T>(value)", static Angle fromDeg(T value)) {
        return Angle(Degree<T>(value));
    }

public:
    // overwite convertXYZSerialized since Angle uses different internal and serialized representation.
    static T convertToSerialized(T value) { return rad2deg<T>(value); }

    static T convertFromSerialized(T value) { return deg2rad<T>(value); }
};

template <typename T, typename SerializerTag>
class IsTransparentSerializable<Angle<T>,SerializerTag> : public std::true_type {};



typedef SignedDegree<int>    SignedDegreei;
typedef SignedDegree<float>  SignedDegreef;
typedef SignedDegree<double> SignedDegreed;
typedef Degree<int>    Degreei;
typedef Degree<float>  Degreef;
typedef Degree<double> Degreed;

typedef SignedRadian<float>  SignedRadianf;
typedef SignedRadian<double> SignedRadiand;
typedef Radian<float>  Radianf;
typedef Radian<double> Radiand;

typedef SignedAngle<float>  SignedAnglef;
typedef SignedAngle<double> SignedAngled;
typedef Angle<float>  Anglef;
typedef Angle<double> Angled;


template<typename T>
inline T smallestAngleDifference(const T& a, const T& b)
{
    return SignedAngle<T>(a).smallestDifference(SignedAngle<T>(b)).value();
}

template<typename T>
inline bool inAngleInterval(T value, T min, T max)
{
    return SignedAngle<T>(value).isInInterval(SignedAngle<T>(min),
                                              SignedAngle<T>(max));
}

template<typename T>
inline bool isInAngleInterval(T value, T min, T max)
{
    return SignedAngle<T>(value).isInInterval(SignedAngle<T>(min),
                                              SignedAngle<T>(max));
}

template <typename T>
Getter<T> rad2degGetter(const T& cref)
{
    return getter<T>([&]{return rad2deg<T>(cref);});
}

struct Deg2RadNonNegativeType { explicit Deg2RadNonNegativeType() = default; };

constexpr Deg2RadNonNegativeType Deg2RadNonNegative;

template <typename T>
Setter<T> deg2radSetter(T& ref)
{
    return setter<T>([&ref](const T& in){ ref = deg2rad<T>(in);});
}

template <typename T>
Setter<T> deg2radSetter(T& ref, Deg2RadNonNegativeType)
{
    // we should throw an exception in error case here as we cannot emit a
    // message including the name of the member/property at this place
    // --> exception allows to catch on a higher level to add/show more information
    return setter<T>([&ref](const T& in){
        if (in < 0) {
            MIRA_THROW(XInvalidParameter,
                       "deg2radSetter: input value must be non-negative.");
        }
        ref = deg2rad<T>(in);
    });
}

template <typename T>
Accessor<Getter<T>, Setter<T>> radAsDegAccessor(T& ref) {
    return makeAccessor(rad2degGetter(ref), deg2radSetter(ref));
}

template <typename T>
Accessor<Getter<T>, Setter<T>> radAsDegAccessor(T& ref, Deg2RadNonNegativeType) {
    return makeAccessor(rad2degGetter(ref), deg2radSetter(ref, Deg2RadNonNegative));
}


}


// specialization of std::abs for all Angle types
namespace std {
template <typename T, typename UnitTag, typename Derived>
inline Derived abs(const mira::AngleBase<T,UnitTag,Derived>& other) {
    return Derived(std::abs(other.value()));
}
}


#endif