227 lines
6.8 KiB
C++
227 lines
6.8 KiB
C++
/*
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*
|
|
*/
|
|
|
|
#ifndef _GAZEBO_ANGLE_HH_
|
|
#define _GAZEBO_ANGLE_HH_
|
|
|
|
#include <math.h>
|
|
#include <ignition/math/Angle.hh>
|
|
#include <iostream>
|
|
#include "gazebo/util/system.hh"
|
|
|
|
/// \brief Macro that converts radians to degrees
|
|
/// \param[in] radians
|
|
/// \return degrees
|
|
#define GZ_RTOD(r) ((r) * 180 / M_PI)
|
|
|
|
/// \brief Converts degrees to radians
|
|
/// \param[in] degrees
|
|
/// \return radians
|
|
#define GZ_DTOR(d) ((d) * M_PI / 180)
|
|
|
|
/// \brief Macro tha normalizes an angle in the range -Pi to Pi
|
|
/// \param[in] angle
|
|
/// \return the angle, in range
|
|
#define GZ_NORMALIZE(a) (atan2(sin(a), cos(a)))
|
|
|
|
namespace gazebo
|
|
{
|
|
/// \ingroup gazebo_math
|
|
|
|
/// \brief Math namespace
|
|
namespace math
|
|
{
|
|
/// \addtogroup gazebo_math Math
|
|
/// \{
|
|
|
|
/// \class Angle Angle.hh math/gzmath.hh
|
|
/// \brief An angle and related functions.
|
|
class GZ_MATH_VISIBLE Angle
|
|
{
|
|
/// \brief math::Angle(0)
|
|
public: static const Angle Zero;
|
|
|
|
/// \brief math::Angle(M_PI)
|
|
public: static const Angle Pi;
|
|
|
|
/// \brief math::Angle(M_PI * 0.5)
|
|
public: static const Angle HalfPi;
|
|
|
|
/// \brief math::Angle(M_PI * 2)
|
|
public: static const Angle TwoPi;
|
|
|
|
/// \brief Constructor
|
|
public: Angle();
|
|
|
|
/// \brief Copy Constructor
|
|
/// \param[in] _radian Radians
|
|
public: Angle(double _radian);
|
|
|
|
/// \brief Copy constructor
|
|
/// \param[in] _angle Angle to copy
|
|
public: Angle(const Angle &_angle);
|
|
|
|
/// \brief Ignition copy constructor
|
|
/// \param[in] _angle Ignition angle to copy
|
|
public: Angle(const ignition::math::Angle &_angle);
|
|
|
|
/// \brief Destructor
|
|
public: virtual ~Angle();
|
|
|
|
/// \brief Set the value from an angle in radians
|
|
/// \param[in] _radian Radian value
|
|
public: void SetFromRadian(double _radian);
|
|
|
|
/// \brief Set the value from an angle in degrees
|
|
/// \param[in] _degree Degree value
|
|
public: void SetFromDegree(double _degree);
|
|
|
|
/// \brief Get the angle in radians
|
|
/// \return double containing the angle's radian value
|
|
public: double Radian() const;
|
|
|
|
/// \brief Get the angle in degrees
|
|
/// \return double containing the angle's degree value
|
|
public: double Degree() const;
|
|
|
|
/// \brief Normalize the angle in the range -Pi to Pi
|
|
public: void Normalize();
|
|
|
|
/// \brief Dereference operator
|
|
/// \return Double containing the angle's radian value
|
|
public: inline double operator*() const { return value; }
|
|
/// \brief Substraction, result = this - _angle
|
|
/// \param[in] _angle Angle for substraction
|
|
/// \return the new angle
|
|
public: Angle operator-(const Angle &_angle) const;
|
|
|
|
/// \brief Addition operator, result = this + _angle
|
|
/// \param[in] _angle Angle for addition
|
|
/// \return the new angle
|
|
public: Angle operator+(const Angle &_angle) const;
|
|
|
|
/// \brief Multiplication operator, result = this * _angle
|
|
/// \param[in] _angle Angle for multiplication
|
|
/// \return the new angle
|
|
public: Angle operator*(const Angle &_angle) const;
|
|
|
|
/// \brief Division, result = this / _angle
|
|
/// \param[in] _angle Angle for division
|
|
/// \return the new angle
|
|
public: Angle operator/(const Angle &_angle) const;
|
|
|
|
/// \brief Subtraction set, this = this - _angle
|
|
/// \param[in] _angle Angle for subtraction
|
|
/// \return angle
|
|
public: Angle operator-=(const Angle &_angle);
|
|
|
|
/// \brief Addition set, this = this + _angle
|
|
/// \param[in] _angle Angle for addition
|
|
/// \return angle
|
|
public: Angle operator+=(const Angle &_angle);
|
|
|
|
/// \brief Multiplication set, this = this * _angle
|
|
/// \param[in] _angle Angle for multiplication
|
|
/// \return angle
|
|
public: Angle operator*=(const Angle &_angle);
|
|
|
|
/// \brief Division set, this = this / _angle
|
|
/// \param[in] _angle Angle for division
|
|
/// \return angle
|
|
public: Angle operator/=(const Angle &_angle);
|
|
|
|
/// \brief Convert this angle to an ignition::math::Angle.
|
|
/// \return This Angle as an ignition::math::Angle.
|
|
public: ignition::math::Angle Ign() const;
|
|
|
|
/// \brief Assignment operator
|
|
/// \param[in] _angle Radians
|
|
/// \return The new angle
|
|
public: Angle &operator=(const double &_angle);
|
|
|
|
/// \brief Assignment operator
|
|
/// \param[in] _angle ignition::math::Angle to copy
|
|
/// \return The new angle
|
|
public: Angle &operator=(const ignition::math::Angle &_angle);
|
|
|
|
/// \brief Equality operator, result = this == _angle
|
|
/// \param[in] _angle Angle to check for equality
|
|
/// \return true if this == _angle
|
|
public: bool operator ==(const Angle &_angle) const;
|
|
|
|
/// \brief Inequality
|
|
/// \param[in] _angle Angle to check for inequality
|
|
/// \return true if this != _angle
|
|
public: bool operator!=(const Angle &_angle) const;
|
|
|
|
/// \brief Less than operator
|
|
/// \param[in] _angle Angle to check
|
|
/// \return true if this < _angle
|
|
public: bool operator<(const Angle &_angle) const;
|
|
|
|
/// \brief Less or equal operator
|
|
/// \param[in] _angle Angle to check
|
|
/// \return true if this <= _angle
|
|
public: bool operator<=(const Angle &_angle) const;
|
|
|
|
/// \brief Greater than operator
|
|
/// \param[in] _angle Angle to check
|
|
/// \return true if this > _angle
|
|
public: bool operator>(const Angle &_angle) const;
|
|
|
|
/// \brief Greater or equal operator
|
|
/// \param[in] _angle Angle to check
|
|
/// \return true if this >= _angle
|
|
public: bool operator>=(const Angle &_angle) const;
|
|
|
|
/// \brief Stream insertion operator. Outputs in degrees
|
|
/// \param[in] _out output stream
|
|
/// \param[in] _a angle to output
|
|
/// \return The output stream
|
|
public: friend std::ostream &operator<<(std::ostream &_out,
|
|
const gazebo::math::Angle &_a)
|
|
{
|
|
_out << _a.Radian();
|
|
return _out;
|
|
}
|
|
|
|
/// \brief Stream extraction operator. Assumes input is in degrees
|
|
/// \param in input stream
|
|
/// \param pt angle to read value into
|
|
/// \return The input stream
|
|
public: friend std::istream &operator>>(std::istream &_in,
|
|
gazebo::math::Angle &_a)
|
|
{
|
|
// Skip white spaces
|
|
_in.setf(std::ios_base::skipws);
|
|
_in >> _a.value;
|
|
return _in;
|
|
}
|
|
|
|
/// The angle in radians
|
|
private: double value;
|
|
};
|
|
|
|
/// \}
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|