misc.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016,
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Christoph Rösmann
00037  *********************************************************************/
00038 
00039 #ifndef MISC_H
00040 #define MISC_H
00041 
00042 #include <Eigen/Core>
00043 #include <boost/utility.hpp>
00044 #include <boost/type_traits.hpp>
00045 
00046 
00047 namespace teb_local_planner
00048 {
00049 
00050 #define SMALL_NUM 0.00000001
00051 
00052 
00053 
00054 
00062 inline bool is_close(double a, double b, double epsilon = 1e-4) 
00063 { 
00064   return std::fabs(a - b) < epsilon; 
00065 }
00066 
00072 inline double average_angles(const std::vector<double>& angles)
00073 {
00074   double x=0, y=0;
00075   for (std::vector<double>::const_iterator it = angles.begin(); it!=angles.end(); ++it)
00076   {
00077       x += cos(*it);
00078       y += sin(*it);
00079   }
00080   if(x == 0 && y == 0)
00081       return 0;
00082   else
00083       return std::atan2(y, x);
00084 }
00085 
00087 inline bool smaller_than_abs(double i, double j) {return std::fabs(i)<std::fabs(j);}
00088 
00089 
00095 inline double fast_sigmoid(double x)
00096 {
00097   return x / (1 + fabs(x));
00098 }
00099 
00106 template <typename P1, typename P2>
00107 inline double distance_points2d(const P1& point1, const P2& point2)
00108 {
00109   return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) );
00110 }
00111 
00112 
00119 template <typename V1, typename V2>
00120 inline double cross2d(const V1& v1, const V2& v2)
00121 {
00122      return v1.x()*v2.y() - v2.x()*v1.y();
00123 }
00124 
00134 template<typename T>
00135 inline const T& get_const_reference(const T* ptr) {return *ptr;}
00136 
00147 template<typename T>
00148 inline const T& get_const_reference(const T& val, typename boost::disable_if<boost::is_pointer<T> >::type* dummy = 0) {return val;}
00149 
00150 } // namespace teb_local_planner
00151 
00152 #endif /* MISC_H */


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Sat Jun 8 2019 20:21:34