misc.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann
37  *********************************************************************/
38 
39 #ifndef MISC_H
40 #define MISC_H
41 
42 #include <Eigen/Core>
43 #include <boost/utility.hpp>
44 #include <boost/type_traits.hpp>
45 
46 
47 namespace teb_local_planner
48 {
49 
50 #define SMALL_NUM 0.00000001
51 
53 enum class RotType { left, none, right };
54 
62 inline bool is_close(double a, double b, double epsilon = 1e-4)
63 {
64  return std::fabs(a - b) < epsilon;
65 }
66 
72 inline double average_angles(const std::vector<double>& angles)
73 {
74  double x=0, y=0;
75  for (std::vector<double>::const_iterator it = angles.begin(); it!=angles.end(); ++it)
76  {
77  x += cos(*it);
78  y += sin(*it);
79  }
80  if(x == 0 && y == 0)
81  return 0;
82  else
83  return std::atan2(y, x);
84 }
85 
87 inline bool smaller_than_abs(double i, double j) {return std::fabs(i)<std::fabs(j);}
88 
89 
95 inline double fast_sigmoid(double x)
96 {
97  return x / (1 + fabs(x));
98 }
99 
106 template <typename P1, typename P2>
107 inline double distance_points2d(const P1& point1, const P2& point2)
108 {
109  return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) );
110 }
111 
112 
119 template <typename V1, typename V2>
120 inline double cross2d(const V1& v1, const V2& v2)
121 {
122  return v1.x()*v2.y() - v2.x()*v1.y();
123 }
124 
134 template<typename T>
135 inline const T& get_const_reference(const T* ptr) {return *ptr;}
136 
147 template<typename T>
148 inline const T& get_const_reference(const T& val, typename boost::disable_if<boost::is_pointer<T> >::type* dummy = 0) {return val;}
149 
150 } // namespace teb_local_planner
151 
152 #endif /* MISC_H */
const T & get_const_reference(const T *ptr)
Helper function that returns the const reference to a value defined by either its raw pointer type or...
Definition: misc.h:135
double cross2d(const V1 &v1, const V2 &v2)
Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) ...
Definition: misc.h:120
TFSIMD_FORCE_INLINE const tfScalar & y() const
double fast_sigmoid(double x)
Calculate a fast approximation of a sigmoid function.
Definition: misc.h:95
RotType
Symbols for left/none/right rotations.
Definition: misc.h:53
double epsilon
TFSIMD_FORCE_INLINE const tfScalar & x() const
double average_angles(const std::vector< double > &angles)
Return the average angle of an arbitrary number of given angles [rad].
Definition: misc.h:72
bool smaller_than_abs(double i, double j)
Small helper function: check if |a|<|b|.
Definition: misc.h:87
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
bool is_close(double a, double b, double epsilon=1e-4)
Check whether two variables (double) are close to each other.
Definition: misc.h:62
double distance_points2d(const P1 &point1, const P2 &point2)
Calculate Euclidean distance between two 2D point datatypes.
Definition: misc.h:107
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10