Skip to content
Snippets Groups Projects
Commit 7abe9b8c authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved a bug whne checking the side of the lateral offset.

Added a function to check whether a given input lane is connected to any output lane.
parent de667f61
No related branches found
No related tags found
1 merge request!2Solved a bug when creating a new geometry object: the sale factor is...
......@@ -63,6 +63,7 @@ class CRoadSegment
void unlink_lanes(unsigned int lane1,unsigned int lane2);
bool are_lanes_linked(unsigned int lane1,unsigned int lane2);
void get_connectivity_matrix(Eigen::MatrixXd &connectivity);
bool is_input_lane_connected(unsigned int lane);
double get_length(void);
bool get_point_at(double distance, double lateral_offset,TPoint &point);
bool get_closest_point(TPoint &target_point,TPoint &closest_point,double &distance,double &lateral_offset,double angle_tol=std::numeric_limits<double>::max());
......
......@@ -350,6 +350,21 @@ void CRoadSegment::get_connectivity_matrix(Eigen::MatrixXd &connectivity)
connectivity=this->connectivity;
}
bool CRoadSegment::is_input_lane_connected(unsigned int lane)
{
unsigned int num=0;
if(lane>=this->connectivity.rows())
throw CException(_HERE_,"Invalid lane index");
for(unsigned int i=0;i<this->connectivity.cols();i++)
if(this->connectivity(lane,i)==1.0)
num++;
if(num>0)
return true;
else
return false;
}
double CRoadSegment::get_length(void)
{
return this->spline.get_length();
......@@ -388,7 +403,7 @@ bool CRoadSegment::get_closest_point(TPoint &target_point,TPoint &closest_point,
{
lateral_offset=sqrt(pow(target_point.x-closest_point.x,2.0)+pow(target_point.y-closest_point.y,2.0));
/* check the side */
cross=(this->end_point.x-this->start_point.x)*(closest_point.y-this->start_point.y)-(this->end_point.y-this->start_point.y)*(closest_point.x-this->start_point.x);
cross=(this->end_point.x-this->start_point.x)*(target_point.y-this->start_point.y)-(this->end_point.y-this->start_point.y)*(target_point.x-this->start_point.x);
if(cross<0.0)
lateral_offset*=-1.0;
return true;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment