geometry_tools.h
Go to the documentation of this file.
```00001 #ifndef GEOMETRY_TOOLS_H_
00002 #define GEOMETRY_TOOLS_H_
00003
00004 #include <geometry_msgs/Polygon.h>
00005 #include <geometry_msgs/Point.h>
00006 #include <costmap_2d/costmap_2d.h>
00007
00008 namespace frontier_exploration{
00009
00016   template<typename T, typename S>
00017   double pointsDistance(const T &one, const S &two){
00018       return sqrt(pow(one.x-two.x,2.0) + pow(one.y-two.y,2.0) + pow(one.z-two.z,2.0));
00019   }
00020
00026   double polygonPerimeter(const geometry_msgs::Polygon &polygon){
00027
00028       double perimeter = 0;
00029       if(polygon.points.size()   > 1)
00030       {
00031         for (int i = 0, j = polygon.points.size() - 1; i < polygon.points.size(); j = i++)
00032         {
00033           perimeter += pointsDistance(polygon.points[i], polygon.points[j]);
00034         }
00035       }
00036       return perimeter;
00037   }
00038
00046   template<typename T, typename S>
00047   bool pointsNearby(const T &one, const S &two, const double &proximity){
00048       return pointsDistance(one, two) <= proximity;
00049   }
00050
00057   template<typename T>
00058   bool pointInPolygon(const T &point, const geometry_msgs::Polygon &polygon){
00059       int cross = 0;
00060       for (int i = 0, j = polygon.points.size()-1; i < polygon.points.size(); j = i++) {
00061           if ( ((polygon.points[i].y > point.y) != (polygon.points[j].y>point.y)) &&
00062               (point.x < (polygon.points[j].x-polygon.points[i].x) * (point.y-polygon.points[i].y) / (polygon.points[j].y-polygon.points[i].y) + polygon.points[i].x) ){
00063               cross++;
00064           }
00065       }
00066       return bool(cross % 2);
00067   }
00068
00075   template<typename T, typename S>
00076   double yawOfVector(const T &origin, const S &end){
00077
00078       double delta_x, delta_y;
00079       delta_x = end.x - origin.x;
00080       delta_y = end.y - origin.y;
00081
00082       double yaw = atan(delta_x/delta_y);
00083
00084       if(delta_x < 0){
00085           yaw = M_PI-yaw;
00086       }
00087
00088       return yaw;
00089   }
00090
00091 }
00092
00093 #endif
```

frontier_exploration
Author(s): Paul Bovbel
autogenerated on Fri Aug 28 2015 10:43:32