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