PolyOps.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*- */
00002 /*
00003  *  Copyright (C) 2007, 2010 David Li, Patrick Beeson, Bartley Gillen,
00004  *                           Jack O'Quin
00005  *
00006  *  License: Modified BSD Software License Agreement
00007  * 
00008  *  $Id: PolyOps.h 1667 2011-08-17 16:14:11Z cor0505@aol.com $
00009  */
00010 
00019 #ifndef __POLYOPS_H__
00020 #define __POLYOPS_H__
00021 
00022 
00023 #include <utility>
00024 #include <math.h>
00025 #include <vector>
00026 #include <set>
00027 #include <map>
00028 
00029 #include <art_msgs/ArtLanes.h>
00030 #include <art_map/coordinates.h>
00031 #include <art_map/types.h>
00032 
00038 class poly
00039 {
00040 public:
00041 
00042   // Each polygon is a quadrilateral.  The four vertex points are
00043   // ordered relative to the heading of the lane containing it.
00044   MapXY p1;    // bottom left
00045   MapXY p2;    // top left
00046   MapXY p3;    // top right
00047   MapXY p4;    // bottom right
00048 
00049   // average of right and left boundary headings
00050   float heading;
00051 
00052   // Middle of the polygon
00053   MapXY midpoint;
00054 
00055   // Length of the polygon
00056   float length;
00057 
00058   poly_id_t poly_id;                    // unique MapLanes ID
00059 
00060   bool is_stop;                         // this poly has a stop waypoint
00061   bool is_transition;                   // not a lane polygon, no waypoint
00062 
00063   // if true, both start_way and end_way are the contained waypoint
00064   bool contains_way;
00065 
00066   ElementID start_way;
00067   ElementID end_way;
00068 
00069   Lane_marking  left_boundary;
00070   Lane_marking  right_boundary;
00071 
00073   poly() {};
00074 
00076   poly(const art_msgs::ArtQuadrilateral &msg)
00077   {
00078     p1 = MapXY(msg.poly.points[art_msgs::ArtQuadrilateral::bottom_left]);
00079     p2 = MapXY(msg.poly.points[art_msgs::ArtQuadrilateral::top_left]);
00080     p3 = MapXY(msg.poly.points[art_msgs::ArtQuadrilateral::top_right]);
00081     p4 = MapXY(msg.poly.points[art_msgs::ArtQuadrilateral::bottom_right]);
00082     midpoint = MapXY(msg.midpoint);
00083 
00084     heading = msg.heading;
00085     length = msg.length;
00086     poly_id = msg.poly_id;
00087 
00088     is_stop = msg.is_stop;
00089     is_transition = msg.is_transition;
00090     contains_way = msg.contains_way;
00091 
00092     start_way = ElementID(msg.start_way);
00093     end_way = ElementID(msg.end_way);
00094 
00095     left_boundary = Lane_marking(msg.left_boundary.lane_marking);
00096     right_boundary = Lane_marking(msg.right_boundary.lane_marking);
00097   };
00098 
00099   void toMsg(art_msgs::ArtQuadrilateral &msg) {
00100     msg.poly.points.resize(art_msgs::ArtQuadrilateral::quad_size);
00101     p1.toMsg(msg.poly.points[art_msgs::ArtQuadrilateral::bottom_left]);
00102     p2.toMsg(msg.poly.points[art_msgs::ArtQuadrilateral::top_left]);
00103     p3.toMsg(msg.poly.points[art_msgs::ArtQuadrilateral::top_right]);
00104     p4.toMsg(msg.poly.points[art_msgs::ArtQuadrilateral::bottom_right]);
00105     midpoint.toMsg(msg.midpoint);
00106 
00107     msg.heading = heading;
00108     msg.length = length;
00109     msg.poly_id = poly_id;
00110     
00111     msg.is_stop = is_stop;
00112     msg.is_transition = is_transition;
00113     msg.contains_way = contains_way;
00114     
00115     msg.start_way = start_way.toMapID();
00116     msg.end_way = end_way.toMapID();
00117 
00118     msg.left_boundary.lane_marking = left_boundary;
00119     msg.right_boundary.lane_marking = right_boundary;
00120   }
00121 };
00122 
00123 typedef std::vector<poly> poly_list_t;  // polygon vector type
00124 
00125 // Stuff returned from vision..
00126 typedef struct polyUpdate
00127 {
00128   poly_id_t poly_id;                    // unique MapLanes ID
00129   int point_id;
00130   float distance;
00131   float bearing;
00132   float confidence;
00133 } lanes_poly_vision_t;
00134 
00135 
00141 class PolyOps
00142 {
00143  public:
00144   PolyOps();
00145   ~PolyOps();
00146 
00147   int get_waypoint_index(const std::vector<poly> &polys,
00148                          const ElementID& waypoint);
00149 
00150   int getPolyWayPt(const std::vector<poly> &polys,
00151                                 const ElementID& waypoint);
00152     
00153   // add from_polys polygons to to_polys matching from_id and to_id
00154   void add_polys_for_waypts(const std::vector <poly> &from_polys,
00155                             std::vector <poly> &to_polys,
00156                             ElementID from_id, ElementID to_id);
00157 
00158   // add from_polys polygons matching segment and lane to to_polys
00159   void AddTransitionPolys(const std::vector <poly> &from_polys,
00160                           std::vector <poly> &to_polys,
00161                           WayPointNode way0, WayPointNode way1);
00162   
00163   // add from_polys polygons matching segment and lane of waypt id to
00164   // to_polys
00165   void AddLanePolys(const std::vector <poly> &from_polys,
00166                     std::vector <poly> &to_polys, ElementID id);
00167 
00168   void AddLanePolys(const std::vector <poly> &from_polys,
00169                     std::vector <poly> &to_polys, WayPointNode waypt);
00170 
00171   // add from_polys polygons matching segment and lane to to_polys
00172   // in either direction (reverse if direction < 0)
00173   void AddLanePolysEither(const std::vector <poly> &from_polys,
00174                           std::vector <poly> &to_polys, WayPointNode waypt,
00175                           int direction);
00176   
00177   // add from_polys polygons matching segment and lane of waypt id to to_polys,
00178   // searching the list in the reverse direction
00179   void AddReverseLanePolys(const std::vector <poly> &from_polys,
00180                            std::vector <poly> &to_polys, ElementID id);
00181 
00182   void AddReverseLanePolys(const std::vector <poly> &from_polys,
00183                            std::vector <poly> &to_polys, WayPointNode waypt);
00184   
00185   // Collect all polygons of from_poly from start to end from to_polys.
00186   void CollectPolys(const std::vector<poly> &from_polys,
00187                     std::vector<poly> &to_polys,
00188                     unsigned start, unsigned end);
00189 
00190   void CollectPolys(const std::vector<poly> &from_polys,
00191                     std::vector<poly> &to_polys,
00192                     unsigned start);
00193 
00194   void GetPolys(const art_msgs::ArtLanes &lanes, poly_list_t &polyList);
00195 
00196   void GetLanes(poly_list_t &polyList, art_msgs::ArtLanes &lanes);
00197 
00198   // true if curPoly is in the specified segment and lane
00199   // Note: this ignores stop line polygons, we don't want to use them
00200   // for steering.  Why do they interfere?
00201   bool LanePoly(const poly &curPoly, ElementID id);
00202 
00203   bool LanePoly(const poly &curPoly, WayPointNode waypt);
00204 
00205   // true if curPoly connects way0 and way1
00206   bool match_waypt_poly(const poly& curPoly, ElementID way0, ElementID way1)
00207   {
00208     return (ElementID(curPoly.start_way) == way0
00209             && ElementID(curPoly.end_way) == way1);
00210   }
00211 
00212   // true if curPoly contains way0
00213   bool match_waypt_poly(const poly& curPoly, ElementID way)
00214   {
00215     return (ElementID(curPoly.start_way) == way
00216             && ElementID(curPoly.end_way) == way);
00217   }
00218 
00219   // return true if curPoly is an transition polygon leading from way0 to way1
00220   bool MatchTransitionPoly(const poly& curPoly, 
00221                            const WayPointNode& way0, 
00222                            const WayPointNode& way1);
00223 
00224   float PolyHeading(const poly& curPoly);
00225 
00226   // determines if point lies in interior of given polygon points on
00227   // edge segments are considered interior points
00228   bool pointInHull(float x, float y, const poly& p)
00229   {
00230     float minx=p.p1.x;
00231     float maxx=p.p1.x;
00232     float miny=p.p1.y;
00233     float maxy=p.p1.y;
00234     
00235     minx=fminf(fminf(fminf(minx,p.p2.x),p.p3.x),p.p4.x);
00236     miny=fminf(fminf(fminf(miny,p.p2.y),p.p3.y),p.p4.y);
00237     maxx=fmaxf(fmaxf(fmaxf(maxx,p.p2.x),p.p3.x),p.p4.x);
00238     maxy=fmaxf(fmaxf(fmaxf(maxy,p.p2.y),p.p3.y),p.p4.y);
00239 
00240     return (Epsilon::gte(x,minx) && Epsilon::lte(x,maxx) &&
00241             Epsilon::gte(y,miny) && Epsilon::lte(y,maxy));
00242   }
00243   
00244   bool pointOnSegment(float x, float y, MapXY p1, MapXY p2)
00245   {
00246     float minx=fminf(p1.x,p2.x);
00247     float miny=fminf(p1.y,p2.y);
00248     float maxx=fmaxf(p1.x,p2.x);
00249     float maxy=fmaxf(p1.y,p2.y);
00250 
00251     if (Epsilon::gte(x,minx) && Epsilon::lte(x,maxx) &&
00252         Epsilon::gte(y,miny) && Epsilon::lte(y,maxy))
00253       {
00254         float diffy=p2.y-p1.y;
00255         float diffx=p2.x-p1.x;
00256         
00257         float diff2y=y-p1.y;
00258         float diff2x=x-p1.x;
00259         
00260         if (Epsilon::equal(diffx,0.0))
00261           return (Epsilon::equal(diff2x,0.0) &&
00262                   ((diff2y<0) == (diffy<0)));
00263         
00264         if (Epsilon::equal(diff2x,0.0))
00265           return false;
00266         
00267         return (Epsilon::equal(diffy/diffx,diff2y/diff2x));
00268       }
00269     return false;
00270     
00271   }
00272   
00273   bool pointOnEdges(float x, float y, const poly& p)
00274   {
00275     return (pointOnSegment(x, y, MapXY(p.p1), MapXY(p.p2)) ||
00276             pointOnSegment(x, y, MapXY(p.p3), MapXY(p.p2)) ||
00277             pointOnSegment(x, y, MapXY(p.p4), MapXY(p.p3)) ||
00278             pointOnSegment(x, y, MapXY(p.p1), MapXY(p.p4)));
00279   }
00280 
00281   bool pointInPoly(float x, float y, const poly& p)
00282   {
00283     if (!pointInHull(x,y,p))
00284       return false;
00285 
00286     bool odd = false;
00287 
00288     // this is an unrolled version of the standard point-in-polygon algorithm
00289 
00290     if ((p.p1.y < y && p.p2.y >= y) || (p.p2.y < y && p.p1.y >= y))
00291       if (p.p1.x + (y-p.p1.y)/(p.p2.y-p.p1.y)*(p.p2.x-p.p1.x) < x)
00292         odd = !odd;
00293 
00294     if ((p.p2.y < y && p.p3.y >= y) || (p.p3.y < y && p.p2.y >= y))
00295       if (p.p2.x + (y-p.p2.y)/(p.p3.y-p.p2.y)*(p.p3.x-p.p2.x) < x)
00296         odd = !odd;
00297 
00298     if ((p.p3.y < y && p.p4.y >= y) || (p.p4.y < y && p.p3.y >= y))
00299       if (p.p3.x + (y-p.p3.y)/(p.p4.y-p.p3.y)*(p.p4.x-p.p3.x) < x)
00300         odd = !odd;
00301 
00302     if ((p.p4.y < y && p.p1.y >= y) || (p.p1.y < y && p.p4.y >= y))
00303       if (p.p4.x + (y-p.p4.y)/(p.p1.y-p.p4.y)*(p.p1.x-p.p4.x) < x)
00304         odd = !odd;
00305 
00306     if (odd)
00307       return true;
00308 
00309     return pointOnEdges(x, y, p);
00310   
00311   }
00312 
00313   bool pointInPoly(const MapXY& pt, const poly& p)
00314   {
00315     return pointInPoly(pt.x, pt.y, p);
00316   };
00317 
00318   bool pointInPoly(const Polar& polar, const MapPose &origin,
00319                    const poly &p)
00320   {
00321     return pointInPoly(Coordinates::Polar_to_MapXY(polar, origin), p);
00322   };
00323 
00324   //bool pointInPoly(const player_pose2d_t &pose, const poly& p)
00325   //{
00326   //  return pointInPoly(pose.px, pose.py, p);
00327   //};
00328   bool pointInPoly_ratio(float x, float y, const poly& p, float ratio);
00329   bool pointInPoly_ratio(const MapXY& pt, const poly& p, float ratio)
00330   {
00331     return pointInPoly_ratio(pt.x, pt.y, p, ratio);
00332   };
00333   //bool pointInPoly_ratio(const player_pose2d_t &pose,
00334   //                       const poly& p, float ratio)
00335   //{
00336   //  return pointInPoly_ratio(pose.px, pose.py, p, ratio);
00337   //};
00338 
00339   // returns true if point is within epsilon of poly
00340   bool pointNearPoly(double x, double y, const poly& poly, double epsilon)
00341   {
00342     return (getShortestDistToPoly(x, y, poly) < epsilon);
00343   }
00344   bool pointNearPoly(const MapXY& pt, const poly& poly, double epsilon)
00345   {
00346     return (getShortestDistToPoly(pt.x, pt.y, poly) < epsilon);
00347   }
00348   //bool pointNearPoly(const player_pose2d_t &pose, const poly& poly, 
00349   //                 double epsilon)
00350   //{
00351   //  return (getShortestDistToPoly(pose.px, pose.py, poly) < epsilon);
00352   //}
00353 
00361   int getContainingPoly(const std::vector<poly> &polys, float x, float y);
00362   int getContainingPoly(const std::vector<poly>& polys, const MapXY& pt)
00363   {
00364     return getContainingPoly(polys, pt.x, pt.y);
00365   };
00366 
00367   int getContainingPoly(const std::vector<poly>& polys,
00368                         const MapPose &pose)
00369   {
00370     return getContainingPoly(polys, pose.map.x, pose.map.y);
00371   };
00372 
00373   // return containing POLYGON ID, -1 if none in list
00374   poly_id_t getContainingPolyID(const std::vector<poly> &polys,
00375                                 float x, float y)
00376   {
00377     int index = getContainingPoly(polys, x, y);
00378     if (index < 0)
00379       return -1;
00380     else
00381       return polys[index].poly_id;
00382   };
00383   poly_id_t getContainingPolyID(const std::vector<poly>& polys,
00384                                 const MapXY& pt)
00385   {
00386     int index = getContainingPoly(polys, pt);
00387     if (index < 0)
00388       return -1;
00389     else
00390       return polys[index].poly_id;
00391   };
00392   //poly_id_t getContainingPolyID(const std::vector<poly>& polys,
00393   //                            const player_pose2d_t &pose)
00394   //{
00395   //  int index = getContainingPoly(polys, pose);
00396   //  if (index < 0)
00397   //    return -1;
00398   //  else
00399   //    return polys[index].poly_id;
00400   //};
00401   
00402   // return index of curPoly in polys vector, -1 if missing
00403   int getPolyIndex(const std::vector<poly>& polys, const poly& curPoly)
00404   {
00405     int i;
00406     for (i = 0; i < (int) polys.size(); ++i)
00407       {
00408         if (polys.at(i).poly_id == curPoly.poly_id)
00409           return i;
00410       }
00411     return -1;
00412   }
00413 
00414   // copy from_polys polygons to to_polygons after nearest to point
00415   void getRemainingPolys(const std::vector<poly> &from_polys,
00416                          std::vector<poly> &to_polys,
00417                          const MapXY &point);
00418   //void getRemainingPolys(const std::vector<poly> &from_polys,
00419   //                     std::vector<poly> &to_polys,
00420   //                     const player_pose2d_t &pose)
00421   //{
00422   //  getRemainingPolys(from_polys, to_polys, MapXY(pose));
00423   //}
00424 
00425   // if the point lies within the given polygon, the returned distance
00426   // is 0 otherwise, the shortest distance to any edge/vertex of the
00427   // given polygon is returned
00428   float getShortestDistToPoly(float x, float y, const poly& p);
00429   float getShortestDistToPoly(MapXY pt, const poly& p)
00430   {
00431     return getShortestDistToPoly(pt.x, pt.y, p);
00432   }
00433   //float getShortestDistToPoly(const player_pose2d_t &pose, const poly& p)
00434   //{
00435   //  return getShortestDistToPoly(pose.px, pose.py, p);
00436   //}
00437 
00438   // if the point lies within a polygon, that polygon is returned.
00439   // otherwise, the nearest polygon from the list is returned index of
00440   // winning poly within list is stored in index
00441   int getClosestPoly(const std::vector<poly>& polys, float x, float y);
00442   int getClosestPoly(const std::vector<poly>& polys, MapXY pt)
00443   {
00444     return getClosestPoly(polys, pt.x, pt.y);
00445   }
00446   int getClosestPoly(const std::vector<poly>& polys,
00447                      const MapPose &pose)
00448   {
00449     return getClosestPoly(polys, pose.map.x, pose.map.y);
00450   }
00451   //int getClosestPoly(const std::vector<poly>& polys, const Polar& pt, 
00452   //                 player_pose2d_t pose)
00453   //{
00454   //  MapXY mapxy = Coordinates::Polar_to_MapXY(pt, pose);
00455   //  return getClosestPoly(polys, mapxy);
00456   //}
00457 
00458   // Returns index of closest polygon if within given epsilon, -1 otherwise
00459   //int getClosestPolyEpsilon(const std::vector<poly>& polys,
00460   //                          const player_pose2d_t& pose, const float epsilon);
00461 
00462   // if the point lies within a non-transtion polygon, that polygon is returned.
00463   // otherwise, the nearest non-transition polygon from the list is returned.
00464   // index of winning non-transition poly within list is stored in index.
00465   int getClosestNonTransPoly(const std::vector<poly>& polys, float x, float y);
00466   int getClosestNonTransPoly(const std::vector<poly>& polys, MapXY pt)
00467   {
00468     return getClosestNonTransPoly(polys, pt.x, pt.y);
00469   }
00470   //int getClosestNonTransPoly(const std::vector<poly>& polys,
00471   //                           const player_pose2d_t &pose)
00472   //{
00473   //  return getClosestNonTransPoly(polys, pose.px, pose.py);
00474   //}
00475   //int getClosestNonTransPoly(const std::vector<poly>& polys, const Polar& pt, 
00476   //                 player_pose2d_t pose)
00477   //{
00478   //  MapXY mapxy = Coordinates::Polar_to_MapXY(pt, pose);
00479   //  return getClosestNonTransPoly(polys, mapxy);
00480   //}
00481 
00482   // Returns index of closest non-transition polygon if within given epsilon,
00483   // -1 otherwise.
00484   //int getClosestNonTransPolyEpsilon(const std::vector<poly>& polys,
00485   //                                  const player_pose2d_t& pose,
00486   //                                  const float epsilon);
00487                            
00488   // returns an x-y pair representing the midpoint of the 2-3 (top)
00489   // edge of input polygon
00490   MapXY getPolyEdgeMidpoint(const poly& p);
00491 
00492   // Returns the center of the polygon
00493   MapXY centerpoint(const poly& p);
00494 
00495   // Return the length of a polygon
00496   float getLength(const poly& p);
00497 
00498   // returns list of polygons between the polygons containing the two
00499   // given points. assumes polygon list is sorted and that second
00500   // point follows first (e.g. waypoints from navigator Order).
00501   // returned list starts with polygon containing or closest to the
00502   // first point. if second point is not inside a polygon, new list
00503   // will extend to end of the old one
00504   std::vector<poly> getPolysBetweenPoints(const std::vector<poly>& polys, 
00505                                           float x1, float y1, 
00506                                           float x2, float y2);
00507                 
00508   // returns list polygon edge midpoints - ideally these can be used
00509   // by navigator as waypoints
00510   std::vector<MapXY> getPointsFromPolys(const std::vector<poly>& polys);
00511 
00524   int getStartingPoly(const MapPose &pose,
00525                       const std::vector<poly>& polygons,
00526                       float distance,
00527                       float min_heading);
00528   
00529   //ElementID updateLaneLocation(const std::vector<poly>& polygons,
00530   //                             const player_pose2d_t& pose,
00531   //                             const WayPointNode& waypt1,
00532   //                             const WayPointNode& waypt2);
00533 
00534 
00535   // Finds the closest polygons to two points, then finds the length
00536   // in the ordered list of polygons between them.
00537   float distanceAlongLane(const std::vector<poly>& polygons,
00538                           const MapXY& from,
00539                           const MapXY& to);
00540 
00541   std::pair<float, MapXY>
00542   specialDistanceAlongLane(const std::vector<poly>& polygons,
00543                            const MapXY& from,
00544                            const MapXY& to);
00545     
00546   //Finds the distance between the midpoints of two polygons
00547   //float distanceBetweenPolygons(const std::vector<poly>& polygons,
00548   //                            poly from,
00549   //                            poly to);
00550   
00551   // Returns the index of the polygon that is distance downstream
00552   // from the trailing edge of polygons[start_index]. If distance
00553   // is less than the length of polygons[start_index], start_index is returned.
00554   int index_of_downstream_poly(const std::vector<poly>& polygons,
00555                                int start_index,
00556                                float distance);
00557 
00558 
00559   // returns shortest distance from a point to a line segment
00560   float shortestDistToLineSegment(float x, float y, float line_x1, 
00561                                   float line_y1, float line_x2, 
00562                                   float line_y2);
00563 
00564   // returns the average length of the 4 sides of a polygon
00565   float avgLengthOfPolySides(const poly& p);
00566 
00567   // Return a Set of unique lane IDs corresponding to the polys in the list
00568   std::set<ElementID> getPolyLaneIds(const std::vector<poly>& polys);
00569 
00570   // Return a unique lane ID corresponding to the polys/dir given
00571   // (uses transition polygons to determine closest lanes)
00572   // input:  a) neighborhood polygons
00573   //         b) relative flag (used for determining direction)
00574   //              0 relative to lane heading
00575   //              1 relative to pose paramater
00576   //         c) direction (relative to relative flag)
00577   //             +1 for getting left lane ID
00578   //              0 for getting current lane ID
00579   //             -1 for getting right lane ID
00580   //         d) pose
00581   //         e) epsilon in which our closest poly to make observations
00582   //              from must be within
00583   //ElementID getPolyLaneIdDir(const poly_list_t& polys,
00584   //                           const int relative,
00585   //                           const int direction,
00586   //                           const player_pose2d_t &pose,
00587   //                           const float poly_epsilon);
00588 
00589   // Return a unique lane ID corresponding to the polys/dir given
00590   // (does NOT use transition polygons to determine closest lanes)
00591   // input:  a) neighborhood polygons
00592   //         b) relative flag (used for determining direction)
00593   //              0 relative to lane heading
00594   //              1 relative to pose paramater
00595   //         c) direction (relative to relative flag)
00596   //             +1 for getting left lane ID
00597   //              0 for getting current lane ID
00598   //             -1 for getting right lane ID
00599   //         d) pose
00600   //         e) epsilon in which our closest poly to make observations
00601   //              from must be within
00602   //ElementID getNonTransPolyLaneIdDir(const poly_list_t& polys,
00603   //                           const int relative,
00604   //                           const int direction,
00605   //                           const player_pose2d_t &pose,
00606   //                           const float poly_epsilon);
00607 
00608   // Return the polygons in lane corresponding to the polys/dir given
00609   // input:  a) neighborhood polygons
00610   //         b) empty destination polygon vector
00611   //         c) relative flag (used for determining direction)
00612   //              0 relative to lane heading
00613   //              1 relative to pose paramater
00614   //         d) direction (relative to relative flag)
00615   //             +1 for getting left lane
00616   //              0 for getting current lane
00617   //             -1 for getting right lane
00618   //         e) pose
00619 
00620   void getLaneDir(const std::vector<poly>& polys,
00621                   std::vector<poly>& to_polys,
00622                   const int relative,
00623                   const int direction,
00624                   const MapPose &pose);
00625 
00626   // Return the lane polys to the left up to num_lanes away.
00627   // similar interface to getLaneDir()
00628   //void getNumLanesDir(const std::vector<poly>& polys,
00629   //                    std::vector<poly>& to_polys,
00630   //                    const int relative,
00631   //                    const int direction,
00632   //                    const player_pose2d_t &pose,
00633   //                    const unsigned num_lanes);
00634 
00635   // Print useful information of each polygon
00636   void printPolygons(const poly_list_t& polys);
00637 
00638   // Return all the transition polys in the polys passed
00639   poly_list_t getTransitionPolys(const poly_list_t& polys);
00640 
00641   // Return the closest polygon relative to pose in the seg/lane of concern
00642   //poly getClosestPolyInLane(const std::vector<poly>& polys,
00643   //                          const player_pose2d_t& pose,
00644   //                          const ElementID id);
00645 
00646   // Check if polygon is valid
00647   bool isValidPoly(const poly& p);
00648 
00649   // Determine if the pose heading is same dirction as the polygon heading
00650   //bool travelingCorrectDir(const poly& p, const player_pose2d_t& pose);
00651 
00652   MapXY GetClosestPointToLine(MapXY A, MapXY B, 
00653                               MapXY P, bool segmentClamp);
00654 
00655   MapXY midpoint(const MapXY& p1, const MapXY& p2);
00656 
00657   ElementID getReverseLane(const std::vector<poly>& polys,
00658                            const MapPose &pose);
00659 
00660   // true if this_poly is to the left of cur_poly.
00661   bool left_of_poly(const poly &this_poly, const poly &cur_poly)
00662   {
00663     // pose of cur_poly
00664     MapPose cur_pose(cur_poly.midpoint, cur_poly.heading);
00665 
00666     // normalized bearing of this_poly from cur_poly
00667     float theta = Coordinates::bearing(cur_pose, this_poly.midpoint);
00668     return (theta > 0.0);
00669   }
00670 
00671   // true if p1 and p2 are within angle of the same heading
00672   bool same_direction(const poly &p1, const poly &p2, float angle)
00673   {
00674     return (fabs(Coordinates::normalize(p1.heading - p2.heading)) < angle);
00675   }
00676 
00677   // get polys from from_id to to_id and store in to_polys
00678   void getPolysBetweenWayPts(
00679            const std::vector<poly> &from_polys,
00680                                    std::vector<poly> &to_polys,
00681                                    ElementID from_id, ElementID to_id);
00682 
00683   // assumptions:
00684   // - on a 2 lane road
00685   // - way0 and way1 are from comm/nav plan
00686   std::vector<MapXY> getRoadPerimeterPoints(const std::vector<poly>& polys,
00687                                             const ElementID way0,
00688                                             const ElementID way1);
00689 
00690   std::vector<MapXY> getRoadPerimeterPoints(const std::vector<poly>& polys,
00691                                             const ElementID way0);
00692 
00693 
00694  private:
00695 
00696   // TODO: (after Urban Challenge) use Euclidean::DistanceTo()...
00697   // simple distance between two points
00698   float distance(float x1, float y1, float x2, float y2);
00699 
00700   // Total length of midlines of polygons
00701   float length_between_polygons(const std::vector<poly>& polygons,
00702                                 int index1=-1,
00703                                 int index2=-1);
00704 
00705 
00706 };
00707 
00708 #endif


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Fri Jan 3 2014 11:08:34