00001 #ifndef CAJUN_RNDF_H
00002 #define CAJUN_RNDF_H
00003
00004
00005
00006
00010
00011 #define DEFAULT_LANE_WIDTH 4 // meters
00012 #define DEFAULT_SPOT_WIDTH 5 // meters
00013
00014 #include <string>
00015 #include <vector>
00016 #include <cassert>
00017 #include <cstdio>
00018
00019 namespace cajun
00020 {
00021
00022
00023 class proj_t;
00024
00025
00027 class rndf_t
00028 {
00029 private:
00030 class builder_t;
00031 class rndf_tool_t;
00032
00033 public:
00035 class waypoint_id_t
00036 {
00037 public:
00038 waypoint_id_t () { m_id[0] = m_id[1] = m_id[2] = 0; }
00039 waypoint_id_t (unsigned id0_, unsigned id1_, unsigned id2_)
00040 { m_id[0] = id0_; m_id[1] = id1_; m_id[2] = id2_; }
00041
00042 unsigned operator[] (unsigned i_) const { return m_id[i_]; }
00043
00044 bool operator< (waypoint_id_t const &v_) const
00045 {
00046 for (unsigned i = 0; i < 3; i++)
00047 if (m_id[i] != v_.m_id[i])
00048 return m_id[i] < v_.m_id[i];
00049 return false;
00050 }
00051 bool operator!= (waypoint_id_t const &v_) const
00052 {
00053 return m_id[0] != v_.m_id[0] ||
00054 m_id[1] != v_.m_id[1] ||
00055 m_id[2] != v_.m_id[2];
00056 }
00057 bool operator== (waypoint_id_t const &v_) const
00058 {
00059 return m_id[0] == v_.m_id[0] &&
00060 m_id[1] == v_.m_id[1] &&
00061 m_id[2] == v_.m_id[2];
00062 }
00063
00064 private:
00065 unsigned m_id[3];
00066 };
00067
00068
00070 class waypoint_t
00071 {
00072 public:
00073 waypoint_t (rndf_t const *rndf_, waypoint_id_t id_);
00074
00076 waypoint_id_t const &id () const { return m_id; }
00077
00079 double lat () const { return m_lat; }
00081 double lon () const { return m_lon; }
00083 double x () const { return m_x; }
00085 double y () const { return m_y; }
00086
00088 bool is_checkpoint () const { return m_is_checkpoint; }
00090 unsigned checkpoint_id () const { return m_checkpoint_id; }
00091
00093 bool is_entry () const { return ! m_entry.empty (); }
00095 unsigned num_entries () const { return m_entry.size (); }
00099 waypoint_t const &entry (unsigned i_) const
00100 { return m_rndf->waypoint (m_entry[i_]); }
00101
00103 bool is_exit () const { return ! m_exit.empty (); }
00105 unsigned num_exits () const { return m_exit.size (); }
00109 waypoint_t const &exit (unsigned i_) const
00110 { return m_rndf->waypoint (m_exit[i_]); }
00111
00113 bool in_segment () const
00114 { return m_id[0] <= m_rndf->num_segments (); }
00116 bool is_stop () const
00117 { return m_is_stop; }
00118
00120 bool in_zone () const
00121 { return m_id[0] > m_rndf->num_segments (); }
00123 bool is_perimeter () const
00124 { return in_zone () && m_id[1] == 0; }
00126 bool is_spot () const
00127 { return in_zone () && m_id[1] != 0; }
00128
00129 private:
00130 friend class rndf_t;
00131 friend class builder_t;
00132
00133 rndf_t const *m_rndf;
00134
00135 waypoint_id_t m_id;
00136 double m_lat;
00137 double m_lon;
00138 double m_x;
00139 double m_y;
00140
00141 bool m_is_checkpoint;
00142 unsigned m_checkpoint_id;
00143
00144 bool m_is_stop;
00145
00147 std::vector<waypoint_id_t> m_entry;
00149 std::vector<waypoint_id_t> m_exit;
00150 };
00151
00152
00154 class lane_t
00155 {
00156 public:
00158 enum boundary_t
00159 {
00160 DOUBLE_YELLOW, SOLID_YELLOW,
00161 SOLID_WHITE, BROKEN_WHITE,
00162 UNKNOWN
00163 };
00165 enum side_t { LEFT, RIGHT };
00166
00168 enum direction_t { PRIMARY, SECONDARY, INVALID };
00169
00170 lane_t (rndf_t const *rndf_,
00171 unsigned segment_id_, unsigned id_) :
00172 m_rndf (rndf_),
00173 m_segment_id (segment_id_),
00174 m_id (id_),
00175 m_width (DEFAULT_LANE_WIDTH),
00176 m_valid_width (false),
00177 m_direction (INVALID)
00178 {
00179 m_boundary[0] = m_boundary[1] = UNKNOWN;
00180 }
00181
00183 unsigned segment_id () const { return m_segment_id; }
00185 unsigned id () const { return m_id; }
00186
00188 double width () const { return m_width; }
00189
00190
00191 bool is_valid_width () const { return m_valid_width; }
00194
00195 boundary_t boundary (side_t side_) const
00196 { return m_boundary[side_]; }
00197
00199 direction_t direction () const { return m_direction; }
00200
00202 unsigned num_checkpoints () const
00203 { return m_checkpoint.size (); }
00207 waypoint_t const &checkpoint (unsigned i_) const
00208 {
00209 assert (i_ < num_checkpoints ());
00210 return m_rndf->waypoint
00211 (m_segment_id, m_id, m_checkpoint[i_]);
00212 }
00213
00215 unsigned num_entries () const { return m_entry.size (); }
00219 waypoint_t const &entry (unsigned i_) const
00220 {
00221 assert (i_ < num_entries ());
00222 return m_rndf->waypoint
00223 (m_segment_id, m_id, m_entry[i_]);
00224 }
00225
00227 unsigned num_exits () const { return m_exit.size (); }
00231 waypoint_t const &exit (unsigned i_) const
00232 {
00233 assert (i_ < num_exits ());
00234 return m_rndf->waypoint
00235 (m_segment_id, m_id, m_exit[i_]);
00236 }
00237
00239 unsigned num_stops () const { return m_stop.size (); }
00243 waypoint_t const &stop (unsigned i_) const
00244 {
00245 assert (i_ < num_stops ());
00246 return m_rndf->waypoint
00247 (m_segment_id, m_id, m_stop[i_]);
00248 }
00249
00251 unsigned num_waypoints () const
00252 {
00253 return m_rndf->num_waypoints (m_segment_id, m_id);
00254 }
00258 waypoint_t const &waypoint (unsigned id_) const
00259 { return m_rndf->waypoint (m_segment_id, m_id, id_); }
00260
00261
00262 private:
00263 friend class builder_t;
00264
00265 rndf_t const *m_rndf;
00266
00267 unsigned m_segment_id;
00268 unsigned m_id;
00269
00270 double m_width;
00271 bool m_valid_width;
00272
00273 boundary_t m_boundary[2];
00274
00275 direction_t m_direction;
00276
00278 std::vector<unsigned> m_checkpoint;
00280 std::vector<unsigned> m_entry;
00282 std::vector<unsigned> m_exit;
00284 std::vector<unsigned> m_stop;
00285 };
00286
00287
00289 class segment_t
00290 {
00291 public:
00292 segment_t (unsigned id_) : m_id (id_) {}
00293
00295 unsigned id () const { return m_id; }
00297 std::string name () const { return m_name; }
00298
00300 unsigned num_lanes () const { return m_lane.size (); }
00303 lane_t const &lane (unsigned id_) const
00304 {
00305 assert (id_ > 0 && id_ <= num_lanes ());
00306 return m_lane[id_ - 1];
00307 }
00308
00309 private:
00310 friend class builder_t;
00311
00312 unsigned m_id;
00313
00314 std::string m_name;
00315 std::vector<lane_t> m_lane;
00316 };
00317
00318
00320 class spot_t
00321 {
00322 public:
00323 spot_t (rndf_t const *rndf_, unsigned zone_id_, unsigned id_) :
00324 m_rndf (rndf_),
00325 m_zone_id (zone_id_),
00326 m_id (id_),
00327 m_width (DEFAULT_SPOT_WIDTH),
00328 m_valid_width (false) {}
00329
00331 unsigned zone_id () const { return m_zone_id; }
00333 unsigned id () const { return m_id; }
00334
00336 double width () const { return m_width; }
00337
00338
00339 bool is_valid_width () const { return m_valid_width; }
00340
00342 unsigned num_checkpoints () const
00343 { return m_checkpoint.size (); }
00347 waypoint_t const &checkpoint (unsigned i_) const
00348 {
00349 assert (i_ < num_checkpoints ());
00350 return m_rndf->waypoint
00351 (m_zone_id, m_id, m_checkpoint[i_]);
00352 }
00353
00355 unsigned num_waypoints () const
00356 {
00357 return m_rndf->num_waypoints (m_zone_id, m_id);
00358 }
00362 waypoint_t const &waypoint (unsigned id_) const
00363 { return m_rndf->waypoint (m_zone_id, m_id, id_); }
00364
00365 private:
00366 friend class builder_t;
00367
00368 rndf_t const *m_rndf;
00369
00370 unsigned m_zone_id;
00371 unsigned m_id;
00372
00373 double m_width;
00374 bool m_valid_width;
00375
00376
00378 std::vector<unsigned> m_checkpoint;
00379 };
00380
00381
00383 class zone_t
00384 {
00385 public:
00386 zone_t (rndf_t const *rndf_, unsigned id_) :
00387 m_rndf (rndf_), m_id (id_) {}
00388
00390 unsigned id () const { return m_id; }
00392 std::string const &name () const { return m_name; }
00393
00395 unsigned num_points () const
00396 { return m_rndf->num_waypoints (m_id, 0); }
00400 waypoint_t const &point (unsigned id_) const
00401 { return m_rndf->waypoint (m_id, 0, id_); }
00402
00404 unsigned num_spots () const { return m_spot.size (); }
00408 spot_t const &spot (unsigned id_) const
00409 {
00410 assert (id_ > 0 && id_ <= num_spots ());
00411 return m_spot[id_ - 1];
00412 }
00413
00415 unsigned num_entries () const { return m_entry.size (); }
00419 waypoint_t const &entry (unsigned i_) const
00420 {
00421 assert (i_ < num_entries ());
00422 return m_rndf->waypoint (m_id, 0, m_entry[i_]);
00423 }
00424
00426 unsigned num_exits () const { return m_exit.size (); }
00430 waypoint_t const &exit (unsigned i_) const
00431 {
00432 assert (i_ < num_exits ());
00433 return m_rndf->waypoint (m_id, 0, m_exit[i_]);
00434 }
00435
00436 private:
00437 friend class builder_t;
00438
00439 rndf_t const *m_rndf;
00440
00441 unsigned m_id;
00442
00443 std::string m_name;
00444
00446 std::vector<spot_t> m_spot;
00448 std::vector<unsigned> m_entry;
00450 std::vector<unsigned> m_exit;
00451 };
00452 class intersection_t
00453 {
00454 public:
00455 struct ir_point_t
00456 {
00457 double x;
00458 double y;
00459 };
00460 intersection_t (rndf_t const *rndf_)
00461 { m_rndf = rndf_;}
00462
00464 unsigned num_stop_points () const
00465 { return m_stop_points.size (); }
00467 waypoint_t const stop_point (unsigned id_) const
00468 { return m_rndf->waypoint (m_stop_points [id_]); }
00469
00472 unsigned num_passing_lanes () const
00473 { return m_passing_lane_ids.size (); }
00476 waypoint_t const passing_lane_id (unsigned id_) const
00477 { return m_rndf->waypoint (m_passing_lane_ids [id_]); }
00478 bool part_of_stop_points (const waypoint_id_t &wp_id_) const
00479 {
00480 for (unsigned i=0; i < m_stop_points.size ();
00481 i++)
00482 if (wp_id_ == m_stop_points[i])
00483 return true;
00484 return false;
00485 }
00486 bool part_of_passing_lane(const waypoint_id_t &wp_id_) const
00487 {
00488 for (unsigned i=0;
00489 i < m_passing_lane_ids.size (); i++)
00490 if (wp_id_ == m_passing_lane_ids[i])
00491 return true;
00492 return false;
00493 }
00494 void add_stop_point (waypoint_id_t const &wp_id)
00495 {
00496 m_stop_points.push_back (wp_id);
00497 }
00498 void add_passing_lane (waypoint_id_t const &wp_id)
00499 {
00500 m_passing_lane_ids.push_back (wp_id);
00501 }
00502 void print_stop_points () const;
00503 void print_passing_lanes () const;
00504 void print_ir_points () const;
00505
00506 unsigned num_ir_points () const { return m_ir_points.size (); }
00507 ir_point_t const &ir_point (unsigned id_) const
00508 {
00509 assert (id_ < m_ir_points.size ());
00510 return m_ir_points[id_];
00511 }
00512 void determine_ir_region ();
00513 private:
00514 friend class rndf_t;
00515
00516 rndf_t const *m_rndf;
00517
00518
00519 std::vector<ir_point_t> m_ir_points;
00520
00521
00522
00523 std::vector <waypoint_id_t> m_stop_points;
00524
00525
00526
00527
00528 std::vector <waypoint_id_t> m_passing_lane_ids;
00529 };
00530
00531 struct lane_seg_t
00532 {
00533 waypoint_id_t wp_id_1;
00534 waypoint_id_t wp_id_2;
00535 };
00536
00537 static rndf_t *load (std::string const &path_,
00538 proj_t *proj_, bool relocate_ = false);
00539
00540 #ifdef XML_OK
00541 static rndf_t *create (std::string const &path_,
00542 proj_t *proj_, std::string map_file);
00543 #endif
00544
00546 std::string const &name () const { return m_name; }
00548 std::string const &format_version () const { return m_format_version; }
00550 std::string const &creation_date () const { return m_creation_date; }
00551
00553 unsigned num_segments () const { return m_segment.size (); }
00557 segment_t const &segment (unsigned id_) const
00558 {
00559 assert (id_ > 0 && id_ <= num_segments ());
00560 return m_segment[id_ - 1];
00561 }
00562
00564 unsigned num_zones () const { return m_zone.size (); }
00568 zone_t const &zone (unsigned id_) const
00569 {
00570 assert (id_ > num_segments ());
00571 id_ -= num_segments ();
00572 assert (id_ > 0 && id_ <= num_zones ());
00573 return m_zone[id_ - 1];
00574 }
00575
00576 unsigned num_intersections () const
00577 { return m_intersections.size (); }
00578
00579 intersection_t const &intersection (unsigned id_) const
00580 {
00581 assert (id_ < num_intersections ());
00582 return m_intersections [id_];
00583 }
00584
00588 unsigned num_waypoints (unsigned id0_, unsigned id1_) const
00589 {
00590 assert (id0_ > 0 && id0_ < m_waypoint.size ());
00591 assert (id1_ < m_waypoint[id0_].size ());
00592 return m_waypoint[id0_][id1_].size () - 1;
00593 }
00596 waypoint_t const &waypoint (waypoint_id_t const &id_) const
00597 {
00598 assert (id_[2] > 0 &&
00599 id_[2] <= num_waypoints (id_[0], id_[1]));
00600 return m_waypoint[id_[0]][id_[1]][id_[2]];
00601 }
00606 waypoint_t const &waypoint (unsigned id0_,
00607 unsigned id1_, unsigned id2_) const
00608 {
00609 assert (id2_ > 0 && id2_ <= num_waypoints (id0_, id1_));
00610 return m_waypoint[id0_][id1_][id2_];
00611 }
00612
00614 unsigned num_checkpoints () const { return m_checkpoint.size (); }
00618 waypoint_t const &checkpoint (unsigned id_) const
00619 {
00620 assert (id_ > 0 && id_ <= num_checkpoints ());
00621 return waypoint (m_checkpoint[id_ - 1]);
00622 }
00627 void relocate_wp (waypoint_id_t id_, double dx_, double dy_,
00628 double heading_diff_, int utm);
00629 void print_all_intersections ();
00630 void determine_intersections ();
00631 void intersection_at (unsigned si, unsigned li, unsigned wi);
00632 bool part_of_some_intersection (rndf_t::waypoint_id_t const &wp_id_,
00633 unsigned &inter_id_) const;
00634 private:
00635 rndf_t () {}
00636
00637 friend class builder_t;
00638 friend class rndf_tool_t;
00639
00640 std::string m_name;
00641 std::string m_format_version;
00642 std::string m_creation_date;
00643
00645 std::vector<segment_t> m_segment;
00647 std::vector<zone_t> m_zone;
00648
00650 std::vector<waypoint_id_t> m_checkpoint;
00651
00653 std::vector<intersection_t> m_intersections;
00654
00660 std::vector< std::vector< std::vector<waypoint_t> > > m_waypoint;
00661
00664 void relocate (double dx_, double dy_);
00665
00666
00667 };
00668
00669
00670 };
00671
00672
00673 #endif
