ZoneOps.cc
Go to the documentation of this file.
00001 
00002 #include <unistd.h>
00003 #include <float.h>
00004 #include <vector>
00005 #include <map>
00006 
00007 #include <art/epsilon.h>
00008 
00009 #include <art_map/ZoneOps.h>
00010 #include <art_map/RNDF.h>
00011 #include <art_map/types.h>
00012 #include <art_map/RNDF.h>
00013 #include <art_map/coordinates.h>
00014 #include <art_map/euclidean_distance.h>
00015 #include <art_map/PolyOps.h>
00016 #include <art_map/Graph.h>
00017 
00018 ZoneManager::ZoneManager(const ZonePerimeter &_zone,
00019                          float _safety_radius,
00020                          float _scale,
00021                          int _max_cells,
00022                          bool _write_graph,
00023                          ElementID _starting_id,
00024                          MapXY _lower_left,
00025                          MapXY _upper_right) :
00026   starting_id(_starting_id),
00027   write_graph(_write_graph),
00028   zone(_zone),
00029   safety_radius(_safety_radius),
00030   ll(_lower_left),
00031   ur(_upper_right),
00032   scale(fmax(_scale, sqrt((fabs(ur.x-ll.x)*fabs(ur.y-ll.y))/(_max_cells))))
00033 {
00034   
00035   perimeter_points.clear();
00036 
00037   for(unsigned i = 0; i < zone.perimeter_points.size(); i++) {
00038     MapXY before(zone.perimeter_points.at(i).map);
00039     MapXY point(zone.perimeter_points.at((i+1)
00040                                          %zone.perimeter_points.size()).map);
00041     ZoneOps::add_densely(perimeter_points, before, point, scale/3);
00042   }
00043 }
00044 
00045 #if 0
00046 void ZoneManager::build_graph(const ObstacleList &obstacles,
00047                               const MapXY &start)
00048 {
00049   //gmanager.clear_grid();
00050 
00051   for(unsigned i = 0; i < perimeter_points.size(); i++)
00052     gmanager.mark_point(perimeter_points[i]);
00053 
00054   for(unsigned i = 0; i < zone.perimeter_points.size(); i++) {
00055     if(zone.perimeter_points.at(i).is_entry ||
00056        zone.perimeter_points.at(i).is_exit)
00057       gmanager.unmark_box(zone.perimeter_points.at(i).map,
00058                           (int)ceil(2.0/scale));
00059   }
00060   
00061   for(unsigned i = 0; i < obstacles.size(); i++) {
00062     gmanager.mark_point(obstacles[i]);
00063   }
00064 
00065   int start_x, start_y = 0;
00066   gmanager.transform(start_x, start_y, start);
00067 
00068   evg_thin thin(gmanager.grid,
00069                 1, FLT_MAX,
00070                 false, true,
00071                 start_x, start_y);
00072   
00073   thin.reset();
00074   thin.set_location(start_x, start_y);
00075   skeleton_type skel=thin.generate_skeleton();
00076 
00077   nodes.clear();
00078   edges.clear();
00079 
00080   for(unsigned i = 0; i < skel.size(); i++) {
00081     WayPointNode node;
00082     node.map = gmanager.reverse_transform(skel[i].x, skel[i].y);
00083     node.index = i;
00084     node.lane_width = (skel[i].radius*scale);
00085     node.id.seg = 0;
00086     node.id.lane = 0;
00087     node.id.pt = i + 1;
00088     nodes.push_back(node);
00089   }
00090 
00091   for(unsigned i = 0; i < skel.size(); i++) {
00092     for(unsigned j = 0; j < skel.at(i).children.size(); j++) {
00093       WayPointEdge edge(nodes.at(i),
00094                         nodes.at(skel.at(i).children.at(j)),
00095                         UNDEFINED,
00096                         UNDEFINED,
00097                         false);
00098       edge.speed_max = edge.speed_min = 0;
00099       // Distance to the nearest obstacle
00100       edge.distance = (skel.at(i).radius*scale);
00101       edges.push_back(edge);
00102       edges.push_back(edge.reverse());
00103     }
00104   }
00105 }
00106 
00107 WayPointNodeList ZoneManager::path_through_zone(const ObstacleList &obstacles,
00108                                                 MapXY start,
00109                                                 MapXY end) {
00110   WayPointNodeList path_nodes;
00111 
00112   ObstacleList filtered_obstacles = ZoneOps::filter_obstacles(obstacles, zone);
00113 
00114   build_graph(filtered_obstacles, start);
00115   ZoneOps::filter_nodes_and_edges(nodes, edges, safety_radius);
00116   unsigned num_nodes = nodes.size();
00117   unsigned num_edges = edges.size();
00118     
00119   WayPointNode c_nodes[num_nodes];
00120   WayPointEdge c_edges[num_edges];
00121   
00122   for(unsigned i = 0; i < num_nodes; i++)
00123     c_nodes[i] = nodes[i];
00124   
00125   for(unsigned i = 0; i < num_edges; i++)
00126     c_edges[i] = edges[i];
00127   
00128   Graph *g = new Graph(num_nodes, num_edges, c_nodes, c_edges);
00129   
00130   ZoneOps::correct_edge_distances(*g);
00131 
00132   if(write_graph)
00133     ZoneOps::print_graph_as_voronoi(*g);
00134   
00135   bool can_go_straight = false;
00136   for(unsigned i = 0; i < g->nodes_size; i++) {
00137     if((Euclidean::DistanceTo(g->nodes[i].map, start) <
00138         g->nodes[i].lane_width) &&
00139        (Euclidean::DistanceTo(g->nodes[i].map, end) <
00140         g->nodes[i].lane_width))
00141       can_go_straight = true;
00142   }
00143   
00144   if(!can_go_straight) {
00145     WayPointNode *start_node = g->get_closest_node_within_radius(start);
00146     WayPointNode *end_node = g->get_closest_node_within_radius(end);
00147     
00148     if(start_node == NULL)
00149       start_node = g->get_closest_node(start);
00150     
00151     if(end_node == NULL)
00152       end_node = g->get_closest_node(end);
00153     
00154     if(start_node != NULL && end_node != NULL) {
00155       int start_index = start_node->index;
00156       int end_index = end_node->index;
00157       
00158       WayPointEdgeList path = GraphSearch::astar_search(*g,
00159                                                         start_index,
00160                                                         end_index);
00161       
00162       path_nodes = GraphSearch::edge_list_to_node_list(*g, path);
00163     }
00164   }
00165   delete g;
00166   return path_nodes;
00167 }
00168 
00169 #define free_and_set_to_null(ptr) { free(ptr); (ptr) = NULL; }
00170 
00171 
00172 int ZoneOps::filter_nodes_and_edges(WayPointNodeList &nodes,
00173                                     WayPointEdgeList &edges,
00174                                     float safety_radius) {
00175   WayPointNodeList::iterator node = nodes.begin();
00176   while(node < nodes.end()) {
00177     if(node->lane_width < safety_radius) {
00178       WayPointEdgeList::iterator edge = edges.begin();
00179       while(edge < edges.end()) {
00180         if(edge->distance < safety_radius ||
00181            edge->startnode_index == node->index ||
00182            edge->endnode_index == node->index)
00183           edges.erase(edge);
00184         else
00185           edge++;
00186       }
00187       nodes.erase(node);
00188     }
00189     else
00190       node++;
00191   }
00192   return 0;
00193 }
00194 
00195 void ZoneOps::print_graph_as_voronoi(Graph &graph) {
00196   std::map<int,int> index_map;
00197   FILE *fh = fopen("/tmp/graph.v.node","w");
00198   fprintf(fh, "%d 2 1 0\n", graph.nodes_size);
00199   for(int i = 0; i < (int) graph.nodes_size; i++) {
00200     fprintf(fh, "%d %.8f %.8f %.8f\n",
00201             i, 
00202             graph.nodes[i].map.x,
00203             graph.nodes[i].map.y,
00204             graph.nodes[i].lane_width);
00205     index_map[graph.nodes[i].index] = i;
00206   }
00207   fclose(fh);
00208 
00209   fh = fopen("/tmp/graph.v.edge","w");
00210   fprintf(fh, "%d 1\n", graph.edges_size);
00211   for(int i = 0; i < (int) graph.edges_size; i++) {
00212     fprintf(fh, "%d %d %d %.8f\n",
00213             i, 
00214             index_map[graph.edges[i].startnode_index],
00215             index_map[graph.edges[i].endnode_index],
00216             graph.edges[i].distance);
00217   }
00218   fclose(fh);
00219 }
00220 
00221 
00222 int ZoneOps::voronoi_from_evg_thin(WayPointNodeList &nodes,
00223                                    WayPointEdgeList &edges,
00224                                    const ZonePerimeter &zone,
00225                                    float safety_radius,
00226                                    const ObstacleList &obstacles,
00227                                    const MapXY &start,
00228                                    bool write_graph,
00229                                    bool write_image,
00230                                    float scale,
00231                                    int max_cells) {
00232   if(zone.perimeter_points.size() < 3)
00233     return 1;
00234   
00235   MapXY ll = zone.perimeter_points[0].map;
00236   MapXY ur = zone.perimeter_points[0].map;
00237   
00238   std::vector<MapXY> points;
00239 
00240   for(unsigned i = 0; i < zone.perimeter_points.size(); i++) {
00241     MapXY before(zone.perimeter_points.at(i).map);
00242     MapXY point(zone.perimeter_points.at((i+1)%zone.perimeter_points.size()).map);
00243     // sampling at scale should be enough, but seeing weird stuff, so
00244     // divide by ten to make sure
00245     add_densely(points, before, point, scale/3);
00246   }
00247 
00248   expand_bounding_box(points, ll, ur);
00249   
00250   float width = fabs(ur.x-ll.x);
00251   float height = fabs(ur.y-ll.y);
00252   
00253   scale = fmax(scale, sqrt((width*height)/(max_cells)));
00254   
00255   grid_manager grid(scale, ll, ur);
00256   
00257   for(unsigned i = 0; i < points.size(); i++)
00258     grid.mark_point(points[i]);
00259 
00260   for(unsigned i = 0; i < zone.perimeter_points.size(); i++) {
00261     if(zone.perimeter_points.at(i).is_entry ||
00262        zone.perimeter_points.at(i).is_exit)
00263       grid.unmark_box(zone.perimeter_points.at(i).map, (int)ceil(2.0/scale));
00264   }
00265   
00266   for(unsigned i = 0; i < obstacles.size(); i++)
00267     grid.mark_point(obstacles[i]);
00268   
00269   int start_x, start_y = 0;
00270   grid.transform(start_x, start_y, start);
00271   
00272   evg_thin thin(grid.grid,
00273                 1, // distance_min
00274                 FLT_MAX, // distance_max
00275                 false, // no pruning
00276                 true, // use "robot close"
00277                 start_x, start_y);
00278   
00279   skeleton_type skel=thin.generate_skeleton();
00280 
00281   /*
00282     if(write_image) {
00283     fileio IO;
00284     IO.save_file(skel,"/tmp/evg.pgm");  
00285     }
00286   */
00287   
00288   for(unsigned i = 0; i < skel.size(); i++) {
00289     WayPointNode node;
00290     node.map = grid.reverse_transform(skel[i].x, skel[i].y);
00291     node.index = i;
00292     node.lane_width = (skel[i].radius*scale);
00293     node.id.seg = 0;
00294     node.id.lane = 0;
00295     node.id.pt = i + 1;
00296     nodes.push_back(node);
00297   }
00298 
00299   for(unsigned i = 0; i < skel.size(); i++) {
00300     for(unsigned j = 0; j < skel.at(i).children.size(); j++) {
00301       WayPointEdge edge(nodes.at(i),
00302                         nodes.at(skel.at(i).children.at(j)),
00303                         UNDEFINED,
00304                         UNDEFINED,
00305                         false);
00306       edge.speed_max = edge.speed_min = 0;
00307       // Distance to the nearest obstacle
00308       edge.distance = (skel.at(i).radius*scale);
00309       edges.push_back(edge);
00310       edges.push_back(edge.reverse());
00311     }
00312   }
00313 
00314   return 0;
00315 }
00316 
00317 
00318 WayPointNodeList ZoneOps::path_through_zone(const ZonePerimeter &zone,
00319                                             float perimeter_sample,
00320                                             float safety_radius,
00321                                             const ObstacleList &obstacles,
00322                                             MapXY start,
00323                                             MapXY end,
00324                                             bool write_graph,
00325                                             bool write_poly,
00326                                             bool write_obstacles,
00327                                             float scale,
00328                                             int max_cells) {
00329   WayPointNodeList path_nodes;
00330   WayPointNodeList nodes;
00331   WayPointEdgeList edges;
00332 
00333   int rv = -1;
00334 
00335   ObstacleList filtered_obstacles = filter_obstacles(obstacles, zone);
00336 
00337   if(0) {
00338   rv = voronoi_from_trilibrary(nodes,
00339                                edges,
00340                                zone,
00341                                perimeter_sample,
00342                                safety_radius,
00343                                filtered_obstacles,
00344                                write_graph,
00345                                write_poly,
00346                                write_obstacles);
00347   } else {
00348     rv = voronoi_from_evg_thin(nodes,
00349                                edges,
00350                                zone,
00351                                safety_radius,
00352                                filtered_obstacles,
00353                                start,
00354                                write_graph,
00355                                write_poly,
00356                                scale,
00357                                max_cells);
00358   }
00359   if(rv == 0) {
00360     filter_nodes_and_edges(nodes, edges, safety_radius);
00361    
00362     unsigned num_nodes = nodes.size();
00363     unsigned num_edges = edges.size();
00364     
00365     WayPointNode c_nodes[num_nodes];
00366     WayPointEdge c_edges[num_edges];
00367     
00368     for(unsigned i = 0; i < num_nodes; i++)
00369       c_nodes[i] = nodes[i];
00370     
00371     for(unsigned i = 0; i < num_edges; i++)
00372       c_edges[i] = edges[i];
00373     
00374     Graph *g = new Graph(num_nodes, num_edges, c_nodes, c_edges);
00375     
00376     build_new_graph(nodes, edges, *g);
00377     
00378     if(write_graph)
00379       print_graph_as_voronoi(*g);
00380     
00381     bool can_go_straight = false;
00382     for(unsigned i = 0; i < g->nodes_size; i++) {
00383       if((Euclidean::DistanceTo(g->nodes[i].map, start) <
00384           g->nodes[i].lane_width) &&
00385          (Euclidean::DistanceTo(g->nodes[i].map, end) <
00386           g->nodes[i].lane_width))
00387         can_go_straight = true;
00388     }
00389     
00390     if(!can_go_straight) {
00391       WayPointNode *start_node = g->get_closest_node_within_radius(start);
00392       WayPointNode *end_node = g->get_closest_node_within_radius(end);
00393       
00394       if(start_node == NULL)
00395         start_node = g->get_closest_node(start);
00396 
00397       if(end_node == NULL)
00398         end_node = g->get_closest_node(end);
00399       
00400       if(start_node != NULL && end_node != NULL) {
00401         int start_index = start_node->index;
00402         int end_index = end_node->index;
00403         
00404         WayPointEdgeList path = GraphSearch::astar_search(*g,
00405                                                           start_index,
00406                                                           end_index);
00407         
00408         path_nodes = GraphSearch::edge_list_to_node_list(*g, path);
00409       }
00410     }
00411     delete g;
00412   }
00413   return path_nodes;
00414 }
00415 #endif
00416 
00417 void ZoneOps::add_densely(std::vector<MapXY> &points,
00418                           const MapXY &p1,
00419                           const MapXY &p2,
00420                           const float &max_spacing) {
00421   
00422   for(float i = 0; i < 1; i += max_spacing/Euclidean::DistanceTo(p1, p2)) {
00423     MapXY n = p1;
00424     n.x += i * (p2.x - p1.x);
00425     n.y += i * (p2.y - p1.y);
00426     points.push_back(n);
00427   }
00428   
00429   /*
00430   if(Euclidean::DistanceTo(p1, p2) < max_spacing)
00431     points.push_back(p2);
00432   else {
00433     MapXY mid((p1.x+p2.x)/2.0, (p1.y+p2.y)/2.0);
00434     add_densely(points, p1, mid, max_spacing);
00435     add_densely(points, mid, p2, max_spacing);
00436   }  
00437   */
00438 }
00439 
00440 #if 0 // maybe later
00441 ObstacleList ZoneOps::filter_obstacles(const ObstacleList &obstacles,
00442                                        const ZonePerimeter &zone)
00443 {
00444   ObstacleList f;
00445   for(unsigned i = 0; i < obstacles.size(); i++) {
00446     if(point_in_zone(zone, obstacles[i]))
00447       f.push_back(obstacles[i]);
00448   }
00449     return f;
00450 }
00451 #endif
00452 
00453 void ZoneOps::build_new_graph(const WayPointNodeList &nodes,
00454                               const WayPointEdgeList &edges,
00455                               Graph& g)
00456 {
00457   
00458   unsigned num_edges = edges.size();
00459 
00460   for(uint i = 0; i < num_edges; i++) {
00461     WayPointNode* start=g.get_node_by_index(g.edges[i].startnode_index);
00462     WayPointNode* end=g.get_node_by_index(g.edges[i].endnode_index);
00463     
00464     if(start != NULL && end != NULL)      
00465       g.edges[i].distance=Euclidean::DistanceTo(start->map,end->map);
00466   }
00467 
00468 }
00469 
00470 #if 0 // maybe later
00471 void ZoneOps::correct_edge_distances(Graph& g)
00472 {
00473   for(uint i = 0; i < g.edges.size(); i++) {
00474     WayPointNode* start=g.get_node_by_index(g.edges[i].startnode_index);
00475     WayPointNode* end=g.get_node_by_index(g.edges[i].endnode_index);
00476     
00477     if(start != NULL && end != NULL)      
00478       g.edges[i].distance=Euclidean::DistanceTo(start->map,end->map);
00479   }
00480 }
00481 
00482 ZonePerimeter ZoneOps::build_fake_zone(const std::vector<MapXY>
00483                                        &points_to_include,
00484                                        float border_width)
00485 {
00486   ZonePerimeter zone;
00487   zone.zone_id = 0;
00488 
00489   if(points_to_include.size() < 1)
00490     return zone;
00491   
00492   MapXY upper_right = points_to_include[0];
00493   MapXY lower_left = points_to_include[0];
00494 
00495   expand_bounding_box(points_to_include, lower_left, upper_right);
00496 
00497   // Expand rectangle by border_width
00498   lower_left.x -= border_width;
00499   lower_left.y -= border_width;
00500   upper_right.x += border_width;
00501   upper_right.y += border_width;
00502 
00503   zone.perimeter_points.push_back(WayPointNode(MapXY(upper_right.x,
00504                                                      lower_left.y)));
00505   zone.perimeter_points.push_back(WayPointNode(upper_right));
00506   zone.perimeter_points.push_back(WayPointNode(MapXY(lower_left.x,
00507                                                      upper_right.y)));
00508   zone.perimeter_points.push_back(WayPointNode(lower_left));
00509 
00510   return zone;
00511 }
00512 
00513 void ZoneOps::expand_bounding_box(const std::vector<MapXY>
00514                                        &points_to_include,
00515                                  MapXY &lower_left,
00516                                  MapXY &upper_right)
00517 {
00518   // Find bounding box
00519   for(unsigned i = 0; i < points_to_include.size(); i++) {
00520     lower_left.x = fmin(points_to_include[i].x, lower_left.x);
00521     lower_left.y = fmin(points_to_include[i].y, lower_left.y);
00522     upper_right.x = fmax(points_to_include[i].x, upper_right.x);
00523     upper_right.y = fmax(points_to_include[i].y, upper_right.y);
00524   }
00525 }
00526 
00527 void ZoneOps::expand_bounding_box_of_waypoints(const std::vector<WayPointNode>
00528                                                &points_to_include,
00529                                                MapXY &lower_left,
00530                                                MapXY &upper_right)
00531 {
00532   // Find bounding box
00533   for(unsigned i = 0; i < points_to_include.size(); i++) {
00534     lower_left.x = fmin(points_to_include[i].map.x, lower_left.x);
00535     lower_left.y = fmin(points_to_include[i].map.y, lower_left.y);
00536     upper_right.x = fmax(points_to_include[i].map.x, upper_right.x);
00537     upper_right.y = fmax(points_to_include[i].map.y, upper_right.y);
00538   }
00539 }
00540 #endif
00541 
00542 WayPointNode ZoneOps::starting_node_for_zone(const ZonePerimeter &zone) 
00543 {
00544   // Return the first entry we find...
00545   for(unsigned i = 0; i < zone.perimeter_points.size(); i++) {
00546     if(zone.perimeter_points[i].is_entry && !zone.perimeter_points[i].is_exit)
00547       return zone.perimeter_points[i];
00548   }
00549 
00550   return WayPointNode();
00551 }
00552 
00553 ZonePerimeter ZoneOps::get_zone_by_id(const ZonePerimeterList &zones,
00554                                       const segment_id_t &zone_id)
00555 {
00556   for(unsigned i = 0; i < zones.size(); i++) {
00557     if(zones[i].zone_id == zone_id)
00558       return zones[i];
00559   }
00560   ZonePerimeter empty;
00561   return empty;
00562 }
00563 
00564 bool ZoneOps::is_a_zone_id(const ZonePerimeterList &zones,
00565                            const segment_id_t &zone_id)
00566 {
00567   for(unsigned i = 0; i < zones.size(); i++) {
00568     if(zones[i].zone_id == zone_id)
00569       return true;
00570   }
00571   return false;
00572 }
00573 
00574 ZonePerimeterList ZoneOps::build_zone_list_from_rndf(RNDF &rndf,
00575                                                      Graph &graph)
00576 {
00577   ZonePerimeterList zl;
00578   std::vector<Zone>::iterator zi;
00579   for(zi = rndf.zones.begin(); zi != rndf.zones.end(); zi++)
00580     {
00581       ZonePerimeter z=build_zone_perimeter_from_zone(graph, *zi);
00582       bool found=false;
00583       for (uint i = 0; i < graph.edges_size; i++)
00584         {       
00585           int index1 = graph.edges[i].startnode_index;
00586           int index2 = graph.edges[i].endnode_index;
00587           if (graph.nodes[index1].id.seg==z.zone_id &&
00588               graph.nodes[index2].id.seg==z.zone_id)
00589             {
00590               z.speed_limit = graph.edges[i].speed_max;
00591               found=true;
00592               break;
00593             }
00594         }
00595       if (!found)
00596         z.speed_limit=DEFAULT_ZONE_SPEED;
00597       
00598       zl.push_back(z);
00599     }
00600   return zl;
00601 }
00602 
00603 ZonePerimeter ZoneOps::build_zone_perimeter_from_zone(Graph &graph,
00604                                                       Zone &zone)
00605 {
00606   ZonePerimeter zp;
00607   zp.zone_id = (segment_id_t)zone.zone_id;
00608   std::vector<Perimeter_Point>::iterator pp;
00609   for(pp = zone.perimeter.perimeterpoints.begin();
00610       pp != zone.perimeter.perimeterpoints.end();
00611       pp++) {
00612     WayPointNode* wpn =
00613       graph.get_node_by_id(ElementID(zp.zone_id, 0, pp->waypoint_id));
00614     if(wpn != NULL) {
00615       zp.perimeter_points.push_back(*wpn);
00616     }
00617   }
00618 
00619   return zp;
00620 }
00621 
00622 void ZoneOps::print_zone_list(const ZonePerimeterList &zones)
00623 {
00624   ROS_DEBUG_STREAM("Number of Zones: " << zones.size());
00625   for(unsigned i = 0; i < zones.size(); i++)
00626     print_zone(zones[i]);
00627 }
00628 
00629 void ZoneOps::print_zone(const ZonePerimeter &zone)
00630 {
00631   printf("Zone ID: %d Perimeter: ", zone.zone_id);
00632   for(unsigned i = 0; i < zone.perimeter_points.size(); i++) {
00633     printf("(%.6f, %.6f), ",
00634            zone.perimeter_points[i].map.x,
00635            zone.perimeter_points[i].map.y);
00636   }
00637   printf("\n");
00638 }
00639 
00640 
00641 segment_id_t ZoneOps::containing_zone(const ZonePerimeterList &zones,
00642                                       const MapXY &point)
00643 {
00644   for(unsigned i = 0; i < zones.size(); i++) {
00645     if(point_in_zone(zones[i], point))
00646       return zones[i].zone_id;
00647   }
00648   return -1;
00649 }
00650 
00651 bool ZoneOps::point_in_zone(const ZonePerimeter &zone,
00652                             const MapXY &point)
00653 {
00654   if(zone.perimeter_points.size() < 3)
00655     return false;
00656 
00657   int intersections = 0;
00658 
00659   for(unsigned i = 0; i < zone.perimeter_points.size(); i++) {
00660     intersections += intersections_of_segment_and_ray_to_right
00661       (zone.perimeter_points[i].map,
00662        zone.perimeter_points[(i+1) % zone.perimeter_points.size()].map,
00663        zone.perimeter_points[(i+2) % zone.perimeter_points.size()].map,
00664        point);
00665   }
00666 
00667   // Returns false is intersections is even, true if odd
00668   return bool(intersections % 2);
00669 }
00670 
00671 int ZoneOps::intersections_of_segment_and_ray_to_right(const MapXY &p1,
00672                                                        const MapXY &p2,
00673                                                        const MapXY &p3,
00674                                                        const MapXY &r)
00675 {
00676   if(Epsilon::equal(p2.y, r.y) && p2.x >= r.x) {
00677     // p2 is on the ray
00678     if ((p1.y > r.y && p3.y > r.y) || (p1.y < r.y && p3.y < r.y))
00679       return 0;
00680     return 1;
00681   }
00682 
00683 
00684   if((p1.x < r.x) && (p2.x < r.x)) return 0;
00685   if((p1.y < r.y) == (p2.y < r.y)) return 0;
00686   if((p1.x > r.x) && (p2.x > r.x)) return 1;
00687   //By this point, we know p1 and p2 are in diagonal coordinates relative to r
00688 
00689   if((p1.x > r.x) && (p1.y < r.y))
00690     return ((p2.x - r.x)*(p1.y - r.y) < (p1.x - r.x)*(p2.y - r.y));
00691 
00692   if((p1.x < r.x) && (p1.y < r.y))
00693     return ((p2.x - r.x)*(p1.y - r.y) < (p1.x - r.x)*(p2.y - r.y));
00694   
00695   return ((p2.x - r.x)*(p1.y - r.y) > (p1.x - r.x)*(p2.y - r.y));
00696 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


art_map
Author(s): David Li, Patrick Beeson, Bartley Gillen, Tarun Nimmagadda, Mickey Ristroph, Michael Quinlan, Jack O'Quin
autogenerated on Tue Sep 24 2013 10:41:51