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 <algorithm>
00043 #include <cmath>
00044 
00045 namespace costmap_converter
00046 {
00047 
00057 template <typename Point, typename LinePoint>
00058 inline double computeDistanceToLine(const Point& point, const LinePoint& line_pt1, const LinePoint& line_pt2)
00059 {
00060     double dx = line_pt2.x - line_pt1.x;
00061     double dy = line_pt2.y - line_pt1.y;
00062     
00063     double length = std::sqrt(dx*dx + dy*dy);
00064     
00065     if (length>0)
00066       return std::abs(dy * point.x - dx * point.y + line_pt2.x * line_pt1.y - line_pt2.y * line_pt1.x) / length;
00067   
00068     return std::sqrt(std::pow(point.x - line_pt1.x, 2) + std::pow(point.y - line_pt1.y, 2));  
00069 }
00070 
00071   
00082 template <typename Point, typename LinePoint>
00083 inline double computeDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL)
00084 {
00085     double dx = line_end.x - line_start.x;
00086     double dy = line_end.y - line_start.y;
00087     
00088     double length = std::sqrt(dx*dx + dy*dy);
00089     
00090     double u = 0;
00091     
00092     if (length>0)
00093      u = ((point.x - line_start.x) * dx + (point.y - line_start.y)*dy) / length;
00094     
00095     if (is_inbetween)
00096       *is_inbetween = (u>=0 && u<=1);
00097   
00098     if (u <= 0)
00099       return std::sqrt(std::pow(point.x-line_start.x,2) + std::pow(point.y-line_start.y,2));
00100     
00101     if (u >= 1)
00102       return std::sqrt(std::pow(point.x-line_end.x,2) + std::pow(point.y-line_end.y,2));
00103     
00104     return std::sqrt(std::pow(point.x - (line_start.x+u*dx) ,2) + std::pow(point.y - (line_start.y+u*dy),2));
00105 }
00106   
00107 
00116 template <typename Point1, typename Point2>  
00117 inline double norm2d(const Point1& pt1, const Point2& pt2)
00118 {
00119   return std::sqrt( std::pow(pt2.x - pt1.x, 2) + std::pow(pt2.y - pt1.y, 2)  );
00120 }
00121 
00131 template <typename Point1, typename Point2>  
00132 inline bool isApprox2d(const Point1& pt1, const Point2& pt2, double threshold)
00133 {
00134   return ( std::abs(pt2.x-pt1.x)<threshold && std::abs(pt2.y-pt1.y)<threshold );
00135 }
00136 
00137 
00138   
00139 } //end namespace teb_local_planner
00140 
00141 #endif /* MISC_H_ */


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat Jun 8 2019 20:53:15