Angle.hpp

Go to the documentation of this file.

#ifndef __BASE_ANGLE_HH__
#define __BASE_ANGLE_HH__

#include <math.h>
#include <iostream>
#include <base/Float.hpp>
#include <base/Eigen.hpp>

#include <vector>

namespace base
{

class Angle
{
public:
    double rad;

    Angle() : rad(base::unknown<double>()) {}
    
protected:
    explicit Angle( double _rad ) : rad(_rad)
    { 
        canonize(); 
    }

    void canonize()
    {
        if( rad > M_PI || rad <= -M_PI )
        {
            double intp;
            const double side = copysign(M_PI,rad);
            rad = -side + 2*M_PI * modf( (rad-side) / (2*M_PI), &intp ); 
        }
    }

public:
    static inline double rad2Deg( double rad )
    {
        return rad / M_PI * 180.0;
    }

    static inline double deg2Rad( double deg )
    {
        return deg / 180.0 * M_PI;
    }

    static inline double normalizeRad( double rad )
    {
        return Angle(rad).rad;
    }

    static inline Angle fromRad( double rad )
    {
        return Angle( rad );
    }

    static inline Angle fromDeg( double deg )
    {
        return Angle( deg / 180.0 * M_PI );
    }

    static inline Angle unknown()
    {
        Angle result;
        result.rad = base::unknown<double>();
        return result;
    }

    static inline Angle Min()
    {
        return Angle( nextafter(-M_PI, 0) );
    }

    static inline Angle Max()
    {
        return Angle(M_PI);
    }

    double inline getRad() const 
    {
        return rad;
    }

    double inline getDeg() const
    {
        return rad / M_PI * 180;
    }

    bool inline isApprox( Angle other, double prec = 1e-5 ) const
    {
        return fabs(Angle(other.rad - rad).getRad()) < prec;
    }

    void operator=(const Angle &other)
    {
        rad = other.rad;
    }
    
    inline bool operator==(const Angle &other ) const
    {
        return this->rad == other.rad;
    }
    
    inline bool operator<(const Angle &other ) const
    {
        return this->rad < other.rad;
    }
    
    inline bool operator>(const Angle &other ) const
    {
        return this->rad > other.rad;
    }

    inline bool operator<=(const Angle &other ) const
    {
        return this->rad <= other.rad;
    }

    inline bool operator>=(const Angle &other ) const
    {
        return this->rad >= other.rad;
    }

    inline Angle &operator+=(const Angle &other )
    {
        this->rad += other.rad;
        canonize();
        return *this;
    }

    inline Angle &operator-=(const Angle &other )
    {
        this->rad -= other.rad;
        canonize();
        return *this;
    }
    
    inline Angle operator+( const Angle &other ) const
    {
        return Angle::fromRad( getRad() + other.getRad() );
    }

    inline Angle operator-( const Angle &other ) const
    {
        return Angle::fromRad( getRad() - other.getRad() );
    }

    inline Angle operator*( const Angle &other ) const
    {
        return Angle::fromRad( getRad() * other.getRad() );
    }

    inline Angle operator*( const double &val ) const
    {
        return Angle::fromRad( getRad() * val );
    }

    inline Angle flipped() const
    {
        return Angle(rad).flip();
    }
    
    inline Angle &flip()
    {
        if(rad < 0)
            rad += M_PI;
        else
            rad -=M_PI;
        return *this;
    }

    static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b);


    static Angle vectorToVector(const base::Vector3d& a, const base::Vector3d& b, const base::Vector3d& positive);
};

static inline Angle operator*( double a, Angle b )
{
    return Angle::fromRad( a * b.getRad() );
}

std::ostream& operator << (std::ostream& os, Angle angle);

class AngleSegment
{
public:
    AngleSegment();

    AngleSegment(const Angle &start, double _width);
    
    bool isInside(const Angle &angle) const;

    bool isInside(const AngleSegment &segment) const;

    bool split(const Angle &angle, AngleSegment &rest)
    {
        return false;
    }

    std::vector<AngleSegment> split(const Angle &angle)
    {
        return std::vector<AngleSegment>();
    }

    std::vector<AngleSegment> getIntersections(const AngleSegment &b) const;

    double getWidth() const
    {
        return width;
    }
    
    base::Angle getStart() const;

    base::Angle getEnd() const;
    
    double width;
    
    double startRad;

    double endRad;
};

std::ostream& operator << (std::ostream& os, AngleSegment seg);

}

#endif