diff --git a/doc/images/ref_system_road_segments.png b/doc/images/ref_system_road_segments.png
index 76a11654f3866c5dcde9985797be37436446f60d..e5143501fe6d2573ec5afede25ddac1ad55e2973 100644
Binary files a/doc/images/ref_system_road_segments.png and b/doc/images/ref_system_road_segments.png differ
diff --git a/doc/opendrive_road_segment.md b/doc/opendrive_road_segment.md
index 4f4b9a71f304ac91de3a0eae2815ce3c156d6543..637367e2f546f71f7e297ab763a944e8aa6d5378 100644
--- a/doc/opendrive_road_segment.md
+++ b/doc/opendrive_road_segment.md
@@ -11,16 +11,16 @@ The next two images show (as transparent blue boxes) the set of COpendriveRoadSe
<img src="images/road_segments_junction.png" alt="COpendriveRoadSegment objects created for a junction of the example road" width="582" height="501">
-Each road segment is assigned an index when it is created which depends only on the order in which they are imported from the Opendrive file (black number in the middle of the blue boxes in the previous image). A part from this index, each road can also be identified by the Opendrive identifier and name.
+Each road segment is assigned a unique index when it is created which depends only on the order in which they are imported from the Opendrive file (black number in the middle of the blue boxes in the previous image). A part from this index, each road can also be identified by the Opendrive identifier and name.
-The second image shows a detail of the COpendriveRoadSegment objects created for lower junction (juntion0). The upper junction (juntion1) is equivalent. In this case the junction roads have a single right lane, so 6 road segments are necessary to connect the 3 roads.
+The second image shows a detail of the COpendriveRoadSegment objects created for lower junction (junction0). The upper junction (junction1) is equivalent. In this case the junction roads have a single right lane, so 6 road segments are necessary to fully connect the 3 roads.
-The connectivity between all road segments is extracted from the following Opendrive information, so it is very important that this information is valid, otherwise, the process of loading the Opendrive description file may file or generate an undesired road.
+The connectivity between all road segments is extracted from the following Opendrive information, so it is very important that this information is valid, otherwise, the process of loading the Opendrive description file may file or generate an undesired results.
-* Road link predecessor
-* Road Link successor
-* Junction connection
-* Junction connection laneLink.
+* Road -> link -> predecessor
+* Road -> link -> successor
+* Junction -> connection
+* Junction -> connection -> laneLink.
The last two points are only necessary for junction roads.
@@ -28,37 +28,43 @@ The last two points are only necessary for junction roads.
The main features of this class are:
-* The geometry of each road segment is described in two different ways.
+* The geometry of each road segment is described in two different ways.
* as a set of [Opendrive geometry classes](), directly imported from the Opendrive file.
- * as a set of [G2 Splines](), generated from the opendrive geometric information. This representation if usefull when searching for the closest road elements or positions.
-* Provides access to all the lanes of the road segment defined as [COpendriveLane]() objects. The segment lanes can be accessed by two different ways:
- * **by id**: in this case the lanes are identified in the same way that Opendrive identifies them: negative integers values are used to identify rigth lanes, and positive values to identify left lanes. These values increase (for left lanes) or decrease (for right lanes) the further they are from the center of the segment.
- * **by the closest position**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road segment lane which is closest to this position. It is possible to get the identifier of the road segment lane of interest or a reference to it.
-* Provides access to all the road segment nodes of the road defined as [COpendriveRoadNode]() objects. The road nodes can be accessed by two different ways:
- * **by index**: which is an arbitray number assigned when the road is loaded.
- * **by the closest position**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road node which is closest to this position. It is possible to get the index of the road node of interest or a reference to it.
-* It is also possible to find the closest position (or positions) to the road independently of the road element where it belongs.
+ * as a set of [G2 Splines](g2_splines.md), generated from the opendrive geometric information.
+* Provides access to all the lanes of the road segment defined as [COpendriveLane](opendrive_lane.md) objects. The road segment lanes can be accessed by two different ways:
+ * **by id**: in this case the lanes are identified in the same way that Opendrive identifies them: negative integers values are used to identify right lanes, and positive values to identify left lanes. These values increase (for left lanes) or decrease (for right lanes) the further they are from the center of the segment.
+ * **by the closest pose**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road segment lane which is closest to this position. It is possible to get the identifier of the road segment lane of interest or a reference to it.
+* Provides access to all the road segment nodes of the road defined as [COpendriveRoadNode](opendrive_road_node.md) objects. The road nodes can be accessed by two different ways:
+ * **by index**: which is the local index of the road node inside the road segment, not the unique index assigned to the road nodes when they are created.
+ * **by the closest pose**: in this case when the position of interest is known (i.e. the position of the car, an obstacle, etc...), it is possible to get the road node which is closest to this position. It is possible to get the index of the road node of interest or a reference to it.
+* It is possible to find the closest position to the road segment independently of the road element where it belongs.
* Provides connectivity information of each road segment with its neightbours. The connecting road segments can be accessed in two different ways:
- * **by index**: which is an arbitray number assigned when the road is loaded.
+ * **by index**: which is the local index of the connecting road segments inside the current road segment, not the unique index assigned to the road segments when they are created.
* **previous** or **next**: in this case only the previous or next segment or segments are accessed, depending on the side of the road of interest.
-* Provides access to all the traffic signs belonging to the road segment defined as [COpendriveSignals]() objects. In this case it is only possible to access them by index.
-* Provides access to all the obstacles belonging to the road segment defined as [COpendriveObjects]() objects. In this case it is only possible to access them by index.
-* Funcions per retornar punts de la corva en funció de la distà ncia
-* Provides a reference to the parent road [COpendriveRoad]() object.
+* Provides access to all the traffic signs belonging to the road segment defined as [COpendriveSignals](opendrive_signals.md) objects. In this case it is only possible to access them by index.
+* Provides access to all the obstacles belonging to the road segment defined as [COpendriveObjects](opendrive_objects.md) objects. In this case it is only possible to access them by index.
+* It is possible to get the pose and curvature at any point on the lane trajectory.
+* Provides a reference to the parent road [COpendriveRoad](opendrive_road.md) object.
+* In case of unexpected errors, this class throws an instance of CException. All other errors are reported to the user.
For further details on the implementation of these features check the doxygen API documentation.
-## Reference systems
+## Closest nodes, lanes, poses and segment distances
-At this point it is important to pay some attention to the reference system used by road segments and how the closest points and distances are computed.
+At this point it is important to pay some attention to how the closest nodes, lanes, poses and segment distances are computed.
In this case, the origin of the reference system is placed at the center lane (middle of the road segment) at one of its edges. The positive x axis (in red) is pointing toward the segment and the positive y axis (in green) is always pointing to the left (the positive z axis would be up). The next figure shows the reference system of two road segment (a straight segment on the left and a curve segment on the right).
-<img src="images/ref_system_road_segments.png" alt="Definition of the reference system used for COpendriveRoadSegment objects" width="1019" height="421">
+<img src="images/ref_system_road_segments.png" alt="Closest poses and segment distances computation for road segments" width="985" height="437">
The left an right lanes are defined with respect to this reference frame for each road segment, so it is possible that left lanes of one segment connect to right lanes of another one, depending on the definition of their reference systems. This definitions coincides with the one used by Opendrive.
-When finding the closest point to a road segment, the desired position of interest (yellow points in the previous image) is projected (closest distance) to the geometry that describes the center lane of the segment (light red point in the previous image labeled S). If the imaginary line between the position of interest and its projection into the segment is not orthogonal to the segment geometry, it is considered that the point of interest does not belong to the road segment, and therefore, the closest point does not exist.
+When finding the closest pose to a road segment, the desired position of interest (yellow points in the previous image) is projected (closest distance) to the geometry that describes the center lane of the segment (light red point in the previous image labeled S). If the imaginary line between the position of interest and its projection into the segment is not orthogonal to the segment geometry, it is considered that the point of interest does not belong to the road segment, and therefore, the closest point does not exist.
+
+In case the closest point exist, its **segment distance** is always computed from the origin of the reference system of the road segment, following the path defined by the road segment geometry (center of the road segment) as shown in the previous image. Do not confuse this distance with the closest euclidean distance between the point of interest and the closest point belonging to the lane.
+
+The closest lane to a given pose is the lane belonging to the road segment that has the smallest euclidean distance to the given pose. Similarly, the closest road node to a given pose is the road node belonging to the closest lane whose lane link has the smallest euclidean distance to the given pose. In both cases the lane/link geometry is used to compute them. See the [COpendriveLane](opendrive_lane.md) and [COpendrveLink](opendrive_link.md) documentations for more details. In the previous image, the closest node and the closest lane are shown in blue.
+
+
-In case the closest point exist, its distance is always computed from the origin of the reference system of the road segment, following the path defined by the road segment geometry (center of the road segment) as shown in the previous image.
diff --git a/include/opendrive_road_segment.h b/include/opendrive_road_segment.h
index 7a635d70b2e91bd11c9b80809a0c30df1ee18efb..c9acd26a80fdbb9e7bb46ea24fcfd9a8ed3629c9 100644
--- a/include/opendrive_road_segment.h
+++ b/include/opendrive_road_segment.h
@@ -23,6 +23,8 @@ class COpendriveRoadSegment
friend class COpendriveRoad;
friend class COpendriveRoadNode;
friend class COpendriveLane;
+ friend class COpendriveSignal;
+ friend class COpendriveObject;
private:
std::map<int,COpendriveLane *> lanes;
std::vector<COpendriveRoadNode *> nodes;
@@ -72,35 +74,365 @@ class COpendriveRoadSegment
bool connects_segments(COpendriveRoadSegment *segment1,COpendriveRoadSegment *segment2);
COpendriveRoadSegment *get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,int node_side,TOpendriveWorldPose *start=NULL,TOpendriveWorldPose *end=NULL) const;
COpendriveRoadSegment *clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref) const;
+ TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
+ TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
public:
+ /**
+ * \brief Returns the Opendrive name
+ *
+ * \return The Opendrive name of the road segment
+ */
std::string get_name(void) const;
+ /**
+ * \brief Returns the Opendirve numeric identifier
+ *
+ * \return The Opendrive numeric identifier of the road segment.
+ */
unsigned int get_id(void) const;
+ /**
+ * \brief Returns the number of right lanes
+ *
+ * This function returns how many lanes there are on the right side of the road
+ * segment. The number of left and right lanes can be different.
+ *
+ * \return This function returns the number of right lanes. The range of
+ * valid indexes goes from -1 to the value returned by this function
+ * negated.
+ */
unsigned int get_num_right_lanes(void) const;
+ /**
+ * \brief Returns the number of left lanes
+ *
+ * This function returns how many lanes there are on the left side of the road
+ * segment. The number of left and right lanes can be different.
+ *
+ * \return This function returns the number of left lanes. The range of
+ * valid indexes goes from 1 to the value returned by this function.
+ */
unsigned int get_num_left_lanes(void) const;
- const COpendriveLane &get_lane(int index) const;
+ /**
+ * \brief Returns a reference to a lane by id
+ *
+ * This function returns a constant reference to a lane of the road segment,
+ * which can not be modified by the user. If the identifier is not valid, this
+ * function will throw an exception.
+ *
+ * \param id is the Opendrive Lane identifier: negative integers values are used
+ * for right lanes, and positive values for left lanes. These values
+ * increase (for left lanes) or decrease (for right lanes) the further
+ * they are from the center lane of the segment.
+ *
+ * \return constant reference to a lane.
+ */
+ const COpendriveLane &get_lane(int id) const;
+ /**
+ * \brief Returns the number of road nodes
+ *
+ * This function returns how many road nodes belong to the road segment.
+ *
+ * \return This function returns the number of road nodes. The range of
+ * valid indexes goes from 0 to the value returned by this function -1.
+ */
unsigned int get_num_nodes(void) const;
+ /**
+ * \brief Returns a reference to a road node by local index
+ *
+ * This function returns a constant reference to the desired road node
+ * object, which can not be modified by the user. If the index is not valid,
+ * this function will throw an exception.
+ *
+ * \param index is the 0 based local index of the desired road node. It is not
+ * the unique index assigned to the road nodes when they are created.
+ *
+ * \return constant reference to the desired road node object.
+ */
const COpendriveRoadNode &get_node(unsigned int index) const;
+ /**
+ * \brief Returns a reference to the parent road
+ *
+ * This function returns a constant reference to the parent road object, which
+ * can not be modified by the user. If the internal parent road is invalid, this
+ * function will throw an exception.
+ *
+ * \return a constant reference to the parent road object.
+ */
const COpendriveRoad &get_parent_road(void) const;
+ /**
+ * \brief Returns the number of road signals
+ *
+ * This function returns how many road signals are placed at this road segment.
+ *
+ * \return This function returns the number of road signals. The range of
+ * valid indexes goes from 0 to the value returned by this function -1.
+ */
unsigned int get_num_signals(void) const;
+ /**
+ * \brief Returns a reference to a road signal by local index
+ *
+ * This function returns a constant reference to the desired road signal
+ * object, which can not be modified by the user. If the index is not valid,
+ * this function will throw an exception.
+ *
+ * \param index is the 0 based local index of the desired road signal.
+ *
+ * \return constant reference to the desired road signal object.
+ */
const COpendriveSignal &get_signal(unsigned int index) const;
+ /**
+ * \brief Returns the number of road objects
+ *
+ * This function returns how many road objects are placed at this road segment.
+ *
+ * \return This function returns the number of road objects. The range of
+ * valid indexes goes from 0 to the value returned by this function -1.
+ */
unsigned int get_num_objects(void) const;
+ /**
+ * \brief Returns a reference to a road object by local index
+ *
+ * This function returns a constant reference to the desired road object
+ * object, which can not be modified by the user. If the index is not valid,
+ * this function will throw an exception.
+ *
+ * \param index is the 0 based local index of the desired road object.
+ *
+ * \return constant reference to the desired road object object.
+ */
const COpendriveObject &get_object(unsigned int index) const;
+ /**
+ * \brief Returns the number of connecting road segments
+ *
+ * This function returns how many road segments are connected to this road
+ * segment. Since the road segments have to possible traffic directions, the
+ * concept of next and previous is only defined when the side of the road is
+ * selected.
+ *
+ * \return This function returns the number of connecting road segments. The
+ * range of valid indexes goes from 0 to the value returned by this
+ * function -1.
+ */
unsigned int get_num_connecting(void) const;
+ /**
+ * \brief Returns a reference to a connecting road segment by local index
+ *
+ * This function returns a constant reference to the desired connecting road
+ * segment object, which can not be modified by the user. If the index is
+ * not valid, this function will throw an exception.
+ *
+ * \param index is the 0 based local index of the desired connecting road
+ * segment.
+ *
+ * \return constant reference to the desired connecting road segment object.
+ */
const COpendriveRoadSegment &get_connecting(unsigned int index) const;
+ /**
+ * \brief returns the length of the road segment
+ *
+ * This function returns the length of the road segment geometry. The road
+ * segment trajectory is represented by Opendive geometry objects and also
+ * by G2 Splines, and it is different from the link and lanes geometries.
+ * Both representations are equivalent.
+ *
+ * \return length of the road segment in meters.
+ */
double get_length(void) const;
+ /**
+ * \brief returns the pose of the road segment at a given point
+ *
+ * This function returns the pose (x and y coordinates and heading) of the center
+ * lane geometry of the road segment at a given distance from the origin point
+ * of the road segment.
+ *
+ * The road segment trajectory is represented by Opendive geometry objects and also
+ * by G2 Splines, and it is different from the link and lanes geometries.
+ * Both representations are equivalent.
+ *
+ * \param length desired distance from the origin point of the road segment in meters.
+ * This distance must be smaller or equal than the actual road segment
+ * length.
+ *
+ * \return the pose at the desired point in the world reference system.
+ */
TOpendriveWorldPose get_pose_at(double length) const;
+ /**
+ * \brief returns the curvature of the road segment at a given point
+ *
+ * This function returns the curvature of the center geometry of the road segment
+ * at a given distance from the origin point of the road segment.
+ *
+ * The road segment trajectory is represented by Opendive geometry objects and also
+ * by G2 Splines, and it is different from the link and lanes geometries.
+ * Both representations are equivalent.
+ *
+ * \param length desired distance from the origin point of the road segment in meters.
+ * This distance must be smaller or equal than the actual road segment
+ * length.
+ *
+ * \return the curvature at the desired point.
+ */
double get_curvature_at(double length) const;
- TOpendriveLocalPose transform_to_local_pose(const TOpendriveTrackPose &track);
- TOpendriveWorldPose transform_to_world_pose(const TOpendriveTrackPose &track);
+ /**
+ * \brief returns the closest node to a given pose
+ *
+ * This functions finds the closest road node belonging to the road segment to a
+ * given pose, and returns it. This function only returns a valid reference when the
+ * given pose coincides with the road segment. Otherwise, it throws an exception.
+ *
+ * \param world given pose to find the closest road node.
+ * \param distance the node distance between the projection of the given pose to
+ * the lane link geometry of the closest road node, and the start
+ * position of the node, following the corresponding lane link geometry.
+ * If the closest road node does not exist this argument is set to the
+ * maximum double value (std::numeric_limits<double>::max())
+ * \param angle_tol maximum heading difference between the given pose and the closest pose
+ * on the lane link geometry of the closest road node.
+ *
+ * \return a constant reference to the closest node which can not be modified by the
+ * user.
+ */
const COpendriveRoadNode &get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+ /**
+ * \brief returns the index of the closest node to a given pose
+ *
+ * This functions finds the closest road node belonging to the road segment to a given
+ * pose, and returns its index. This function only returns a valid index
+ * when the given pose coincides with the road segment.
+ *
+ * \param world given pose to find the closest road node.
+ * \param distance the node distance between the projection of the given pose to
+ * the lane link geometry of the closest road node, and the start
+ * position of the node, following the corresponding lane link geometry.
+ * If the closest road node does not exist this argument is set to the
+ * maximum double value (std::numeric_limits<double>::max())
+ * \param angle_tol maximum heading difference between the given pose and the closest pose
+ * on the lane link geometry of the closest road node.
+ *
+ * \return the index of the closest node or -1 if there is no node close to the given
+ * pose.
+ */
unsigned int get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+ /**
+ * \brief returns the closest lane to a given pose
+ *
+ * This functions finds the closest lane belonging to the road segment to a
+ * given pose, and returns it. This function only returns a valid reference when the
+ * given pose coincides with the road segment. Otherwise, it throws an exception.
+ *
+ * \param world given pose to find the closest lane.
+ * \param distance the lane distance between the projection of the given pose to
+ * the lane geometry, and the start position of the lane, following
+ * its geometry. If the closest lane does not exist this argument
+ * is set to the maximum double value (std::numeric_limits<double>::max())
+ * \param angle_tol maximum heading difference between the given pose and the closest pose
+ * on the lane geometry.
+ *
+ * \return a constant reference to the closest lane which can not be modified by the
+ * user.
+ */
const COpendriveLane &get_closest_lane(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+ /**
+ * \brief returns the id of the closest lane to a given pose
+ *
+ * This functions finds the closest lane belonging to the road segment to a given
+ * pose, and returns its id. This function only returns a valid id
+ * when the given pose coincides with the road segment.
+ *
+ * \param world given pose to find the closest lane.
+ * \param distance the lane distance between the projection of the given pose to
+ * the lane geometry, and the start position of the lane, following
+ * its geometry. If the closest lane does not exist this argument
+ * is set to the maximum double value (std::numeric_limits<double>::max())
+ * \param angle_tol maximum heading difference between the given pose and the closest pose
+ * on the lane geometry.
+ *
+ * \return the id of the closest node or 0 if there is no lane close to the given
+ * pose.
+ */
int get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol=0.1) const;
+ /**
+ * \brief returns the closest pose to a given pose
+ *
+ * This functions finds the closest pose (minimum distance) on the road segment geometry
+ * to a given pose. The closest pose only exist if the imaginary line between the
+ * given pose and its projection into the road segment is orthogonal to the segment geometry.
+ * Otherwise, it is considered that the given pose does not coincide with the road segment.
+ *
+ * \param world given pose to find the closest pose.
+ * \param closest_pose the closest pose to the center lane of the road segment if the
+ * given pose coincides with it. Otherwise the data on this
+ * parameters is not valid.
+ * \param angle_tol maximum heading difference between the given pose and the closest pose
+ * on the road segment geometry.
+ *
+ * \return the road segment distance between the projection of the given pose to the
+ * center lane geometry of the road segment, following its geometry. If the
+ * closest pose does not exist, the maximum double value
+ * (std::numeric_limits<double>::max()) will be returned.
+ */
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol=0.1) const;
- const COpendriveRoadSegment &get_next_segment(int &side,bool &valid) const;
- const COpendriveRoadSegment &get_prev_segment(int &side,bool &valid) const;
+ /**
+ * \brief returns a reference to the next road segments.
+ *
+ * This function returns a constant reference to the set of road segments that will follow
+ * this road segment, depending on the road side of interest.
+ *
+ * \param input_side an integer indicating the road side of interest of the current
+ * road segment. It is negative for the right side and positive for
+ * the left one (similar to the lane id criteria).
+ * \param output_sides output parameter that indicates the side of the next road segments
+ * that connects with the side of interest of the current one. It
+ * is negative for the right side and positive for the left one. If
+ * there are no next road segments, this vector will be empty.
+ *
+ * \return a vector with the constant references of the next road segments. If there
+ * are no next road segments, this vector will be empty.
+ */
+ std::vector<const COpendriveRoadSegment *> get_next_segments(int input_side,std::vector<int> &output_sides) const;
+ /**
+ * \brief returns a reference to the previous road segments.
+ *
+ * This function returns a constant reference to the set of road segments that preceed
+ * this road segment, depending on the road side of interest.
+ *
+ * \param input_side an integer indicating the road side of interest of the current
+ * road segment. It is negative for the right side and positive for
+ * the left one (similar to the lane id criteria).
+ * \param output_sides output parameter that indicates the side of the previous road segments
+ * that connects with the side of interest of the current one. It
+ * is negative for the right side and positive for the left one. If
+ * there are no previous road segments, this vector will be empty.
+ *
+ * \return a vector with the constant references of the previous road segments. If there
+ * are no previous road segments, this vector will be empty.
+ */
+ std::vector<const COpendriveRoadSegment *> get_prev_segments(int input_side,std::vector<int> &output_sides) const;
+ /**
+ * \brief Returns the road segment side of a given pose
+ *
+ * This function returns the road segment side of a given pose. If the given pose
+ * does not coincide with the road segment, an exception will be thrown.
+ *
+ * \return an integer indicating the road side of the given pose. It is negative for
+ * the right side and positive for the left one (similar to the lane id criteria).
+ */
int get_pose_side(TOpendriveWorldPose &pose) const;
+ /**
+ * \brief overloaded stream operator
+ *
+ * This functions streams human readable information about the road segment. This information
+ * includes:
+ * * The name of the connecting road segments.
+ * * The road nodes information.
+ * * The left lanes information.
+ * * The right lanes information.
+ * * The road signals information.
+ * * The road objects information.
+ *
+ */
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment);
+ /**
+ * \brief Destructor
+ */
~COpendriveRoadSegment();
};
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 41e6f6b312e67834399f50da5efe05827ea754f4..3326af63e96977180b265c1d3f819575a063e0da 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -788,32 +788,30 @@ COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,
return new_segment;
}
-const COpendriveRoadSegment &COpendriveRoadSegment::get_next_segment(int &side,bool &valid) const
+std::vector<const COpendriveRoadSegment *> COpendriveRoadSegment::get_next_segments(int input_side,std::vector<int> &output_sides) const
{
- std::vector<COpendriveRoadSegment *> segment_candidates;
- std::vector<int> side_candidates;
+ std::vector<const COpendriveRoadSegment *> segment_candidates;
bool already_present;
- if(side<0)
+ output_sides.clear();
+ if(input_side<0)
{
for(unsigned int i=1;i<=this->get_num_right_lanes();i++)
{
const COpendriveLane &lane=this->get_lane(-i);
- if(lane.get_num_next_lanes()>1)
- throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
- if(lane.get_num_next_lanes()==1)
+ for(unsigned int j=0;j<lane.get_num_next_lanes();j++)
{
already_present=false;
for(unsigned int k=0;k<segment_candidates.size();k++)
- if(segment_candidates[k]==lane.get_next_lane(0).segment)
+ if(segment_candidates[k]==&lane.get_next_lane(j).get_segment())
{
already_present=true;
break;
}
if(!already_present)
{
- segment_candidates.push_back(lane.get_next_lane(0).segment);
- side_candidates.push_back(lane.get_next_lane(0).get_id());
+ segment_candidates.push_back(&lane.get_next_lane(j).get_segment());
+ output_sides.push_back(lane.get_next_lane(j).get_id());
}
}
}
@@ -823,66 +821,51 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_next_segment(int &side,b
for(unsigned int i=1;i<=this->get_num_left_lanes();i++)
{
const COpendriveLane &lane=this->get_lane(i);
- if(lane.get_num_next_lanes()>1)
- throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
- if(lane.get_num_next_lanes()==1)
+ for(unsigned int j=0;j<lane.get_num_next_lanes();j++)
{
already_present=false;
for(unsigned int k=0;k<segment_candidates.size();k++)
- if(segment_candidates[k]==lane.get_next_lane(0).segment)
+ if(segment_candidates[k]==&lane.get_next_lane(j).get_segment())
{
already_present=true;
break;
}
if(!already_present)
{
- segment_candidates.push_back(lane.get_next_lane(0).segment);
- side_candidates.push_back(lane.get_next_lane(0).get_id());
+ segment_candidates.push_back(&lane.get_next_lane(j).get_segment());
+ output_sides.push_back(lane.get_next_lane(j).get_id());
}
}
}
}
- if(segment_candidates.size()==0)
- {
- valid=false;
- return *this;
- }
- else if(segment_candidates.size()>1)
- throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
- else
- {
- valid=true;
- side=side_candidates[0];
- return *segment_candidates[0];
- }
+
+ return segment_candidates;
}
-const COpendriveRoadSegment &COpendriveRoadSegment::get_prev_segment(int &side,bool &valid) const
+std::vector<const COpendriveRoadSegment *> COpendriveRoadSegment::get_prev_segments(int input_side,std::vector<int> &output_sides) const
{
- std::vector<COpendriveRoadSegment *> segment_candidates;
- std::vector<int> side_candidates;
+ std::vector<const COpendriveRoadSegment *> segment_candidates;
bool already_present;
- if(side<0)
+ output_sides.clear();
+ if(input_side<0)
{
for(unsigned int i=1;i<=this->get_num_right_lanes();i++)
{
const COpendriveLane &lane=this->get_lane(-i);
- if(lane.get_num_prev_lanes()>1)
- throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
- if(lane.get_num_prev_lanes()==1)
+ for(unsigned int j=0;j<lane.get_num_next_lanes();j++)
{
already_present=false;
for(unsigned int k=0;k<segment_candidates.size();k++)
- if(segment_candidates[k]==lane.get_prev_lane(0).segment)
+ if(segment_candidates[k]==&lane.get_prev_lane(j).get_segment())
{
already_present=true;
break;
}
if(!already_present)
{
- segment_candidates.push_back(lane.get_prev_lane(0).segment);
- side_candidates.push_back(lane.get_prev_lane(0).get_id());
+ segment_candidates.push_back(&lane.get_prev_lane(j).get_segment());
+ output_sides.push_back(lane.get_prev_lane(j).get_id());
}
}
}
@@ -892,38 +875,25 @@ const COpendriveRoadSegment &COpendriveRoadSegment::get_prev_segment(int &side,b
for(unsigned int i=1;i<=this->get_num_left_lanes();i++)
{
const COpendriveLane &lane=this->get_lane(i);
- if(lane.get_num_prev_lanes()>1)
- throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
- if(lane.get_num_prev_lanes()==1)
+ for(unsigned int j=0;j<lane.get_num_next_lanes();j++)
{
already_present=false;
for(unsigned int k=0;k<segment_candidates.size();k++)
- if(segment_candidates[k]==lane.get_prev_lane(0).segment)
+ if(segment_candidates[k]==&lane.get_prev_lane(j).get_segment())
{
already_present=true;
break;
}
if(!already_present)
{
- segment_candidates.push_back(lane.get_prev_lane(0).segment);
- side_candidates.push_back(lane.get_prev_lane(0).get_id());
+ segment_candidates.push_back(&lane.get_prev_lane(j).get_segment());
+ output_sides.push_back(lane.get_prev_lane(j).get_id());
}
}
}
}
- if(segment_candidates.size()==0)
- {
- valid=false;
- return *this;
- }
- else if(segment_candidates.size()>1)
- throw CException(_HERE_,"Road has multiple paths and no unique subroad exists");
- else
- {
- valid=true;
- side=side_candidates[0];
- return *segment_candidates[0];
- }
+
+ return segment_candidates;
}
@@ -1011,18 +981,18 @@ unsigned int COpendriveRoadSegment::get_num_left_lanes(void) const
return this->num_left_lanes;
}
-const COpendriveLane &COpendriveRoadSegment::get_lane(int index) const
+const COpendriveLane &COpendriveRoadSegment::get_lane(int id) const
{
std::map<int,COpendriveLane *>::const_iterator lane_it;
std::stringstream error;
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
{
- if(lane_it->second->get_id()==index)
+ if(lane_it->second->get_id()==id)
return *lane_it->second;
}
- error << "Invalid lane index (" << index << ") for segment " << this->name;
+ error << "Invalid lane id (" << id << ") for segment " << this->name;
throw CException(_HERE_,error.str());
}