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 Thu Aug 10 2017 03:02:58