Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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 }
00140
00141 #endif