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 #include <costmap_2d/costmap_math.h>
00031
00032 double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
00033 {
00034 double A = pX - x0;
00035 double B = pY - y0;
00036 double C = x1 - x0;
00037 double D = y1 - y0;
00038
00039 double dot = A * C + B * D;
00040 double len_sq = C * C + D * D;
00041 double param = dot / len_sq;
00042
00043 double xx, yy;
00044
00045 if (param < 0)
00046 {
00047 xx = x0;
00048 yy = y0;
00049 }
00050 else if (param > 1)
00051 {
00052 xx = x1;
00053 yy = y1;
00054 }
00055 else
00056 {
00057 xx = x0 + param * C;
00058 yy = y0 + param * D;
00059 }
00060
00061 return distance(pX, pY, xx, yy);
00062 }
00063
00064 bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy)
00065 {
00066 bool c = false;
00067 int i, j, nvert = polygon.size();
00068 for (i = 0, j = nvert - 1; i < nvert; j = i++)
00069 {
00070 float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x;
00071
00072 if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi))
00073 c = !c;
00074 }
00075 return c;
00076 }
00077
00078 bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
00079 {
00080 for (unsigned int i = 0; i < polygon1.size(); i++)
00081 if (intersects(polygon2, polygon1[i].x, polygon1[i].y))
00082 return true;
00083 return false;
00084 }
00085
00086 bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
00087 {
00088 return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
00089 }
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15