Angle.hpp

Go to the documentation of this file.

/*
 * Angle.hpp
 *
 *  Created on: Oct 27, 2011
 *      Author: mriedel
 */

#ifndef ANGLE_HPP_
#define ANGLE_HPP_

#include <telekyb_defines/telekyb_defines.hpp>

#include <math.h>
#include <string>

namespace TELEKYB_NAMESPACE
{

class Angle{
        private:
                double theta;
                static const double M_2PI; /*for efficency*/

        public:
                // These are Static functions that can be used directly
                static inline double norm2Pi(double a) {
                if (a >= M_2PI || a < 0.0) {
                        a = fmod(a, M_2PI); //in [-2*M_PI,2*M_PI]
                        if (a < 0.0)
                                a += M_2PI; //in [0,2*M_PI]
                        if (a >= M_2PI)
                                a -= M_2PI;
                }
                return a;
        }
        static inline double normPi(double a, int opt = 0) {
                if (a > M_PI || a <= -M_PI) {
                        a = fmod(a, M_2PI); //in [-2*M_PI,2*M_PI]

                        if (opt == 0) {
                                if (a <= -M_PI)
                                        a += M_2PI;
                        }
                        if (opt == 1) {
                                if (a < -M_PI)
                                        a += M_2PI;
                        }
                        if (a > M_PI)
                                a -= M_2PI;
                }
                return a;
        }
        static inline double normMinus2Pi(double a) {
                if (a >= 0.0 || a < -M_2PI) {
                        a = fmod(a, M_2PI); //in [-2*M_PI,2*M_PI]
                        if (a < -M_2PI)
                                a += M_2PI; //in [0,2*M_PI]
                        if (a >= 0.0)
                                a -= M_2PI;
                }
                return a;
        }


        Angle() {
                theta = 0.0;
        };
        Angle(const double& t) {
                theta = norm2Pi(t);
        };
        Angle(const Angle& rhs) {
                theta = norm2Pi(rhs.dCast());
        };
        Angle& operator=(const Angle& rhs){
                if (this != &rhs){      // Not necessary in this case but it is useful to don't forget it
                        theta = norm2Pi(rhs.dCast());
                }
                return *this;
        }
        Angle& operator+=(const Angle& a) {
                theta = norm2Pi(theta + a.theta);
                return *this;
        }
        Angle& operator-=(const Angle& other) {
                double myTheta = theta >= other.theta ? theta : theta + 2.0*M_PI;
                theta = norm2Pi(myTheta - other.theta);
                return *this;
        }
        Angle& operator*=(const Angle& a) {
                theta = norm2Pi(theta * a.theta);
                return *this;
        }
        Angle& operator*=(const double& d) {
                theta = norm2Pi(theta * d);
                return *this;
        }
        Angle operator+(const Angle &other) const {
                return Angle(*this) += other;
        }
        Angle operator-(const Angle &other) const {
                return Angle(*this) -= other;
        }
        Angle operator*(const Angle &other) const {
                return Angle(*this) *= other;
        }
        Angle operator*(const double& d) const {
                return (Angle(*this) *= d);
        }
        bool operator==(const Angle &other) const {
                if( (other.dCast()) == theta)   return true;
                else return false;
        }
        bool operator!=(const Angle &other) const {
                return !(*this == other);
        }
        double dCast() const {
                return theta;
        }

        double dCast2Pi() const {
                return norm2Pi(theta);
        }

        double dCastPi(int opt=0) const {
                if(opt==0) return normPi(theta);
                if(opt==1) return normPi(theta,1);

                // never happens
                return normPi(theta);
        }
        double dCastDeg() const {
                return (theta*180.0)/M_PI;
        }

//              UCoordmm dCastHDeg() const {
//                      return (UCoordmm)((theta*18000.0)/M_PI);
//              }
        double alDiff(const Angle &other) const;

        double ccwDiff(const Angle &other) const;

        double cwDiff(const Angle &other) const;

//              bool almostEqual(const Angle& A, const double& toll) const;

        Angle nearestMean(const Angle &other, double w1 = 0.5) const;

        Angle ccwMean(const Angle &other) const;

        Angle cwMean(const Angle &other) const;

        std::string toString(int precision=2) const;
};

}

#endif /* ANGLE_HPP_ */