angle.hpp
Go to the documentation of this file.
1 
8 /*****************************************************************************
9 ** Ifdefs
10 *****************************************************************************/
11 
12 #ifndef ECL_GEOMETRY_ANGLE_HPP_
13 #define ECL_GEOMETRY_ANGLE_HPP_
14 
15 /*****************************************************************************
16 ** Includes
17 *****************************************************************************/
18 
19 #include <cmath>
20 #include <ecl/config/macros.hpp>
21 #include <ecl/linear_algebra.hpp>
22 #include <ecl/type_traits/fundamental_types.hpp>
23 #include <ecl/mpl/enable_if.hpp>
24 #include <ecl/math/constants.hpp>
25 #include "macros.hpp"
26 
27 /*****************************************************************************
28 ** Namespaces
29 *****************************************************************************/
30 
31 namespace ecl {
32 
40 template <typename T>
41 ECL_PUBLIC T radians_to_degrees(const T &radians, typename enable_if<ecl::is_float<T> >::type* dummy = 0) {
42  static const T rads_to_degs = 180.0/pi;
43  return radians*rads_to_degs;
44 }
45 
53 template <typename T>
54 ECL_PUBLIC T degrees_to_radians(const T &degrees, typename enable_if<ecl::is_float<T> >::type* dummy = 0) {
55  static const T degs_to_rads = pi/180.0;
56  return degrees*degs_to_rads;
57 }
58 
68 ecl_geometry_PUBLIC const float& wrap_angle(float &angle);
69 
80 ecl_geometry_PUBLIC float wrap_angle(const float &angle);
90 ecl_geometry_PUBLIC const double& wrap_angle(double &angle);
91 
102 ecl_geometry_PUBLIC double wrap_angle(const double &angle);
103 
104 /*****************************************************************************
105 ** Interface [Angle]
106 *****************************************************************************/
112 template <class T, typename Enable = void>
113 class ECL_LOCAL Angle {
114 private:
121  Angle() {};
122 };
123 
135 template <typename T>
136 class ECL_PUBLIC Angle<T, typename enable_if<is_float<T> >::type> {
137 public:
138  /*********************
139  ** Typedefs
140  **********************/
141  typedef ecl::linear_algebra::Matrix<T,2,2> RotationMatrix;
150  Angle(const T &angle = 0.0) : value(angle) { wrap_angle(value); }
155  Angle(const RotationMatrix &rotation) {
156  value = std::atan2(rotation(1,0),rotation(0,0));
157  }
158 
168  const Angle<T>& operator=(const T &angle);
169  /*********************
170  ** Math Operators
171  **********************/
180  Angle<T> operator+(const T &angle) const;
189  Angle<T> operator+(const Angle<T> &angle) const;
197  void operator+=(const T &angle);
205  void operator+=(const Angle<T> &angle);
214  Angle<T> operator-(const T &angle) const;
223  Angle<T> operator-(const Angle<T> &angle) const;
231  void operator-=(const T &angle);
239  void operator-=(const Angle<T> &angle);
248  Angle<T> operator*(const T &scalar) const;
256  void operator*=(const T &scalar);
257 
258  /*********************
259  ** Casting
260  **********************/
270  operator const T&() const { return value; }
281  T degrees();
282 
287  RotationMatrix rotationMatrix() const {
288  T c = std::cos(value);
289  T s = std::sin(value);
290  return (RotationMatrix() << c, -s, s, c).finished();
291  }
292 
306  static Angle<T> Degrees(const T &angle);
320  static Angle<T> Radians(const T &angle);
321 
322 private:
323  T value;
324 };
325 
330 /*****************************************************************************
331 ** Implementation
332 *****************************************************************************/
333 
334 template <typename T>
335 T Angle<T, typename enable_if<is_float<T> >::type>::degrees() {
336  return radians_to_degrees(value);
337 }
338 
339 template <typename T>
340 const Angle<T>& Angle<T, typename enable_if<is_float<T> >::type>::operator=(const T &angle) {
341  value = angle;
342  wrap_angle(value);
343  return *this;
344 }
345 
346 /*****************************************************************************
347 ** Operators
348 *****************************************************************************/
349 
350 template <typename T>
351 Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator+(const T &angle) const {
352  return Angle<T>(wrap_angle(value+angle));
353 }
354 
355 template <typename T>
356 Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator+(const Angle<T> &angle) const {
357  return Angle<T>(wrap_angle(value+angle.value));
358 }
359 
360 template <typename T>
361 void Angle<T, typename enable_if<is_float<T> >::type>::operator+=(const T &angle) {
362  value += angle;
363  wrap_angle(value);
364 }
365 
366 template <typename T>
367 void Angle<T, typename enable_if<is_float<T> >::type>::operator+=(const Angle<T> &angle) {
368  value += angle.value;
369  wrap_angle(value);
370 }
371 
372 template <typename T>
373 Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator-(const T &angle) const {
374  return Angle<T>(wrap_angle(value-angle));
375 }
376 
377 template <typename T>
378 Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator-(const Angle<T> &angle) const {
379  return Angle<T>(wrap_angle(value-angle.value));
380 }
381 
382 template <typename T>
383 void Angle<T, typename enable_if<is_float<T> >::type>::operator-=(const T &angle) {
384  value -= angle;
385  wrap_angle(value);
386 }
387 
388 template <typename T>
389 void Angle<T, typename enable_if<is_float<T> >::type>::operator-=(const Angle<T> &angle) {
390  value -= angle.value;
391  wrap_angle(value);
392 }
393 
394 template <typename T>
395 Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::operator*(const T &scalar) const {
396  return Angle<T>(wrap_angle(scalar*value));
397 }
398 
399 template <typename T>
400 void Angle<T, typename enable_if<is_float<T> >::type>::operator*=(const T &scalar) {
401  value *= scalar;
402  wrap_angle(value);
403 }
404 
405 /*****************************************************************************
406 ** Statics
407 *****************************************************************************/
408 
409 template <typename T>
410 Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::Degrees(const T &angle) {
411  return Angle<T>(degrees_to_radians(angle));
412 }
413 
414 template <typename T>
415 Angle<T> Angle<T, typename enable_if<is_float<T> >::type>::Radians(const T &angle) {
416  return Angle<T>(angle);
417 }
422 } // namespace ecl
423 
424 #endif /* ECL_GEOMETRY_ANGLE_HPP_ */
TFSIMD_FORCE_INLINE Vector3 operator*(const Matrix3x3 &m, const Vector3 &v)
TFSIMD_FORCE_INLINE Quaternion operator-(const Quaternion &q)
XmlRpcServer s
ecl_geometry_PUBLIC T radians_to_degrees(const T &radians, typename enable_if< ecl::is_float< T > >::type *dummy=0)
Converts radians to degrees and returns the result.
Definition: angle.hpp:41
Default action for detection of a fundamental float type (false).
double const pi
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
ecl_geometry_PUBLIC T degrees_to_radians(const T &degrees, typename enable_if< ecl::is_float< T > >::type *dummy=0)
Converts degrees to radians and returns the result.
Definition: angle.hpp:54
#define ecl_geometry_PUBLIC
#define ECL_LOCAL
TFSIMD_FORCE_INLINE Vector3 & operator-=(const Vector3 &v)
TFSIMD_FORCE_INLINE Vector3 & operator+=(const Vector3 &v)
TFSIMD_FORCE_INLINE Vector3 & operator*=(const tfScalar &s)
ecl_geometry_PUBLIC const float & wrap_angle(float &angle)
Wrap the angle on -pi,pi (float types).
#define ECL_PUBLIC
Parent template definition for angles.
Definition: angle.hpp:113
const GenericPointer< typename T::ValueType > T2 value
TFSIMD_FORCE_INLINE Vector3 operator+(const Vector3 &v1, const Vector3 &v2)
Enables the SFINAE concept.
Definition: enable_if.hpp:67


xbot_node
Author(s): Roc, wangpeng@droid.ac.cn
autogenerated on Sat Oct 10 2020 03:28:13