diff --git a/include/junction.h b/include/junction.h index d7c7cc2a7de862a84b22cb7a0e9b3cf71fb15daa..58ff7efd2392b3d0e9b500a66e95906b1538788f 100644 --- a/include/junction.h +++ b/include/junction.h @@ -16,6 +16,7 @@ class CJunction friend class CRoad; friend class CRoadMap; friend class CRoadSegment; + friend class COpendriveJunction; private: unsigned int id; double resolution; @@ -57,9 +58,11 @@ class CJunction CRoadMap &get_parent_roadmap(void); unsigned int get_num_incomming_roads(void); CRoad &get_incomming_road_by_id(unsigned int id); + unsigned int get_incomming_road_index_by_id(unsigned int id); bool has_incomming_road(unsigned int id); unsigned int get_num_outgoing_roads(void); CRoad &get_outgoing_road_by_id(unsigned int id); + unsigned int get_outgoing_road_index_by_id(unsigned int id); bool has_outgoing_road(unsigned int id); void add_incomming_road(CRoad *road); void remove_incomming_road_by_id(unsigned int id); @@ -69,7 +72,11 @@ class CJunction void unlink_roads_by_id(unsigned int incomming_road,unsigned int outgoing_road); bool are_roads_linked_by_id(unsigned int incomming_road,unsigned int outgoing_road); void link_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); + void link_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road); + void link_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road); void unlink_lanes_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); + void unlink_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road); + void unlink_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road); bool are_lanes_linked_by_id(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out); void get_connectivity_matrix(Eigen::MatrixXi &connectivity,std::vector<unsigned int> &incomming_id_map,std::vector<unsigned int> &outgoing_id_map); /* geometry */ diff --git a/src/junction.cpp b/src/junction.cpp index dff89aa2ff92ababc12de8d9e3b828997d200620..bc5c9a24c617822fda61ddaf6f0a54ed4161b201 100644 --- a/src/junction.cpp +++ b/src/junction.cpp @@ -89,6 +89,11 @@ CRoad &CJunction::get_incomming_road_by_id(unsigned int id) return *this->get_incomming_road_by_index(index); } +unsigned int CJunction::get_incomming_road_index_by_id(unsigned int id) +{ + return CRoad::get_index_by_id(this->incomming_roads,id); +} + bool CJunction::has_incomming_road(unsigned int id) { if(CRoad::get_index_by_id(this->incomming_roads,id)==(unsigned int)-1) @@ -109,6 +114,11 @@ CRoad *CJunction::get_outgoing_road_by_index(unsigned int index) return this->outgoing_roads[index]; } +unsigned int CJunction::get_outgoing_road_index_by_id(unsigned int id) +{ + return CRoad::get_index_by_id(this->outgoing_roads,id); +} + CRoad &CJunction::get_outgoing_road_by_id(unsigned int id) { unsigned int index; @@ -413,7 +423,6 @@ CRoadSegment *CJunction::link_point_to_point(TPoint &incomming_point,unsigned in return new_segment; } - void CJunction::link_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out) { CRoadSegment *segment; @@ -441,6 +450,31 @@ void CJunction::link_lanes_by_id(unsigned int incomming_road,unsigned int lane_i this->link_lanes_by_index(incomming_index,lane_in,outgoing_index,lane_out); } +void CJunction::link_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int num_lanes; + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + + num_lanes=std::min(this->incomming_roads[incomming_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_num_lanes()); + for(unsigned int i=0;i<num_lanes;i++) + this->link_lanes_by_index(incomming_index,i,outgoing_index,i); +} + +void CJunction::link_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + + for(unsigned int i=0;i<this->incomming_roads[incomming_index]->get_num_lanes();i++) + for(unsigned int j=0;j<this->outgoing_roads[outgoing_index]->get_num_lanes();j++) + this->link_lanes_by_index(incomming_index,i,outgoing_index,j); +} + void CJunction::unlink_lanes_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out) { CRoadSegment *segment; @@ -468,6 +502,31 @@ void CJunction::unlink_lanes_by_id(unsigned int incomming_road,unsigned int lane this->unlink_lanes_by_index(incomming_index,lane_in,outgoing_index,lane_out); } +void CJunction::unlink_same_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int num_lanes; + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + + num_lanes=std::min(this->incomming_roads[incomming_index]->get_num_lanes(),this->outgoing_roads[outgoing_index]->get_num_lanes()); + for(unsigned int i=0;i<num_lanes;i++) + this->unlink_lanes_by_index(incomming_index,i,outgoing_index,i); +} + +void CJunction::unlink_full_lanes_by_id(unsigned int incomming_road,unsigned int outgoing_road) +{ + unsigned int incomming_index,outgoing_index; + + incomming_index=CRoad::get_index_by_id(this->incomming_roads,incomming_road); + outgoing_index=CRoad::get_index_by_id(this->outgoing_roads,outgoing_road); + + for(unsigned int i=0;i<this->incomming_roads[incomming_index]->get_num_lanes();i++) + for(unsigned int j=0;j<this->outgoing_roads[outgoing_index]->get_num_lanes();j++) + this->unlink_lanes_by_index(incomming_index,i,outgoing_index,j); +} + bool CJunction::are_lanes_linked_by_index(unsigned int incomming_road,unsigned int lane_in,unsigned int outgoing_road,unsigned int lane_out) { CRoadSegment *segment;