| 52 | #include "costmap_math.h" |
| 53 | namespace roborts_costmap { |
| 54 | double Distance2Line(double pX, double pY, double x0, double y0, double x1, double y1) { |
| 55 | double A = pX - x0; |
| 56 | double B = pY - y0; |
| 57 | double C = x1 - x0; |
| 58 | double D = y1 - y0; |
| 59 | |
| 60 | double dot = A * C + B * D; |
| 61 | double len_sq = C * C + D * D; |
| 62 | double param = dot / len_sq; |
| 63 | |
| 64 | double xx, yy; |
| 65 | |
| 66 | if (param < 0) { |
| 67 | xx = x0; |
| 68 | yy = y0; |
| 69 | } else if (param > 1) { |
| 70 | xx = x1; |
| 71 | yy = y1; |
| 72 | } else { |
| 73 | xx = x0 + param * C; |
| 74 | yy = y0 + param * D; |
| 75 | } |
| 76 | |
| 77 | return distance(pX, pY, xx, yy); |
| 78 | } |
| 79 | |
| 80 | bool Intersect(std::vector<geometry_msgs::Point> &polygon, float testx, float testy) { |
| 81 | bool c = false; |
no test coverage detected