QuadrilateralOps.cc
Go to the documentation of this file.
00001 #include <art_map/types.h>
00002 #include <art_observers/QuadrilateralOps.h>
00003 #include <art_map/PolyOps.h> 
00004 
00005 namespace quad_ops {
00006 // determines if point lies in interior of given polygon points on
00007   // edge segments are considered interior points
00008   bool pointInHull(float x, float y, const geometry_msgs::Point32 *p1, const geometry_msgs::Point32 *p2, const geometry_msgs::Point32 *p3, const geometry_msgs::Point32 *p4) {
00009     float minx=p1->x;
00010     float maxx=p1->x;
00011     float miny=p1->y;
00012     float maxy=p1->y;
00013     
00014     minx=fminf(fminf(fminf(minx,p2->x),p3->x),p4->x);
00015     miny=fminf(fminf(fminf(miny,p2->y),p3->y),p4->y);
00016     maxx=fmaxf(fmaxf(fmaxf(maxx,p2->x),p3->x),p4->x);
00017     maxy=fmaxf(fmaxf(fmaxf(maxy,p2->y),p3->y),p4->y);
00018 
00019     return (Epsilon::gte(x,minx) && Epsilon::lte(x,maxx) &&
00020             Epsilon::gte(y,miny) && Epsilon::lte(y,maxy));
00021   }
00022 
00023   // Determines in a point inside the ArtQuadrilateral. I called it quick
00024   // as I'm not checking the edges, so we could miss a small percentage
00025   // of points.
00026   bool quickPointInPoly(float x, float y, const art_msgs::ArtQuadrilateral& p) {
00027     const geometry_msgs::Point32 *p1 = &p.poly.points[0];
00028     const geometry_msgs::Point32 *p2 = &p.poly.points[1];
00029     const geometry_msgs::Point32 *p3 = &p.poly.points[2];
00030     const geometry_msgs::Point32 *p4 = &p.poly.points[3];
00031 
00032     if (!pointInHull(x,y,p1,p2,p3,p4))
00033       return false;
00034     
00035     bool odd = false;
00036     
00037     // this is an unrolled version of the standard point-in-polygon algorithm
00038 
00039     if ((p1->y < y && p2->y >= y) || (p2->y < y && p1->y >= y))
00040       if (p1->x + (y-p1->y)/(p2->y-p1->y)*(p2->x-p1->x) < x)
00041         odd = !odd;
00042     
00043     if ((p2->y < y && p3->y >= y) || (p3->y < y && p2->y >= y))
00044       if (p2->x + (y-p2->y)/(p3->y-p2->y)*(p3->x-p2->x) < x)
00045         odd = !odd;
00046     
00047     if ((p3->y < y && p4->y >= y) || (p4->y < y && p3->y >= y))
00048       if (p3->x + (y-p3->y)/(p4->y-p3->y)*(p4->x-p3->x) < x)
00049         odd = !odd;
00050     
00051     if ((p4->y < y && p1->y >= y) || (p1->y < y && p4->y >= y))
00052       if (p4->x + (y-p4->y)/(p1->y-p4->y)*(p1->x-p4->x) < x)
00053         odd = !odd;
00054     
00055     if (odd)
00056       return true;
00057 
00058     return false;
00059   }
00060 
00061   // Unrolled version on pointInPoly, but this time it only checks then inner
00062   // 'ratio' percentage of the polygon.
00063   bool quickPointInPolyRatio(float x, float y, const art_msgs::ArtQuadrilateral& p, float ratio) {
00064     const float diff=(1-ratio)/2;
00065  
00066     geometry_msgs::Point32 p1 = p.poly.points[0];
00067     geometry_msgs::Point32 p2 = p.poly.points[1];
00068     geometry_msgs::Point32 p3 = p.poly.points[2];
00069     geometry_msgs::Point32 p4  = p.poly.points[3];
00070 
00071     // Ratio reduction  
00072     p1.x=p1.x+(p4.x-p1.x)*diff;
00073     p4.x=p1.x+(p4.x-p1.x)*(1-diff);
00074     p1.y=p1.y+(p4.y-p1.y)*diff;
00075     p4.y=p1.y+(p4.y-p1.y)*(1-diff);     
00076 
00077     p2.x=p2.x+(p3.x-p2.x)*diff;
00078     p3.x=p2.x+(p3.x-p2.x)*(1-diff);
00079     p2.y=p2.y+(p3.y-p2.y)*diff;
00080     p3.y=p2.y+(p3.y-p2.y)*(1-diff);
00081 
00082     // Normal checks
00083     if (!pointInHull(x,y,&p1,&p2,&p3,&p4))
00084       return false;
00085     
00086     bool odd = false;
00087     
00088     // this is an unrolled version of the standard point-in-polygon algorithm
00089 
00090     if ((p1.y < y && p2.y >= y) || (p2.y < y && p1.y >= y))
00091       if (p1.x + (y-p1.y)/(p2.y-p1.y)*(p2.x-p1.x) < x)
00092         odd = !odd;
00093     
00094     if ((p2.y < y && p3.y >= y) || (p3.y < y && p2.y >= y))
00095       if (p2.x + (y-p2.y)/(p3.y-p2.y)*(p3.x-p2.x) < x)
00096         odd = !odd;
00097     
00098     if ((p3.y < y && p4.y >= y) || (p4.y < y && p3.y >= y))
00099       if (p3.x + (y-p3.y)/(p4.y-p3.y)*(p4.x-p3.x) < x)
00100         odd = !odd;
00101     
00102     if ((p4.y < y && p1.y >= y) || (p1.y < y && p4.y >= y))
00103       if (p4.x + (y-p4.y)/(p1.y-p4.y)*(p1.x-p4.x) < x)
00104         odd = !odd;
00105     
00106     if (odd)
00107       return true;
00108 
00109     return false;
00110   }
00111   
00112   // This function returns an ArtLanes containing all the
00113   // ArtQuadrilaterals in 'quads' that are satisfied by the 'filter'
00114   // being passed in
00115   art_msgs::ArtLanes filterLanes(const art_msgs::ArtQuadrilateral& base_quad,
00116                                  const art_msgs::ArtLanes& quads,
00117                                  bool(*filter)(const art_msgs::ArtQuadrilateral&, const art_msgs::ArtQuadrilateral&))
00118   {
00119     art_msgs::ArtLanes filtered;
00120     size_t num_quads = quads.polygons.size();
00121     for (size_t i=0; i<num_quads; i++) {
00122       const art_msgs::ArtQuadrilateral* p= &(quads.polygons[i]);
00123       if (filter(base_quad,quads.polygons[i]))  {
00124         filtered.polygons.push_back(*p);
00125       }
00126     }
00127     return filtered;
00128   }
00129   
00130   // This function returns an ArtLanes containing all the 
00131   // ArtQuadrilaterals in 'quads' that are in the adjacent lane
00132   art_msgs::ArtLanes filterAdjacentLanes(MapPose &pose,
00133                                  const art_msgs::ArtLanes& quads,
00134                                  const int lane)
00135   {
00136     PolyOps polyOps;
00137     poly_list_t allPolygons, to_polys;
00138     polyOps.GetPolys(quads, allPolygons);
00139     polyOps.getLaneDir(allPolygons, to_polys, 0, lane, pose);
00140     art_msgs::ArtLanes adjacentPolys;
00141     polyOps.GetLanes(to_polys, adjacentPolys);
00142     return adjacentPolys;
00143     
00144   }
00145 }


art_observers
Author(s): Michael Quinlan, Jack O'Quin, Corbyn Salisbury
autogenerated on Fri Jan 3 2014 11:09:22