diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index c5625b06fb8c79f91eed62c1bb234183d622d70a..1436a113df687f98954ee859b7b3b118457c5351 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -36,7 +36,6 @@ class COpendriveRoadNode
void add_link(COpendriveLink *link);
void remove_link(COpendriveLink *link);
bool has_link(COpendriveLink *link);
- bool has_link_with(COpendriveRoadNode *node);
COpendriveLink *get_link_with(COpendriveRoadNode *node);
void set_resolution(double res);
void set_scale_factor(double scale);
@@ -62,10 +61,11 @@ class COpendriveRoadNode
unsigned int get_num_links(void) const;
const COpendriveLink &get_link(unsigned int index) const;
const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
+ bool has_link_with(const COpendriveRoadNode &node) const;
unsigned int get_closest_link(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
double get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes=false,double angle_tol=0.1) const;
double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes=false,double angle_tol=0.1) const;
- void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes,double angle_tol=0.1) const;
+ void get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes=false,double angle_tol=0.1) const;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
};
diff --git a/src/g2_spline.cpp b/src/g2_spline.cpp
index acb107ca394e750d47e5a172a0d31b97b0f31fbf..5709b60c69db30232591fc6cdceefc1b9aec2d24 100644
--- a/src/g2_spline.cpp
+++ b/src/g2_spline.cpp
@@ -801,7 +801,7 @@ double CG2Spline::get_max_curvature(double max_error,unsigned int max_iter)
{
start_point=this->get_max_sample_point(boost::bind(&CG2Spline::curvature_pow2,this,_1));
if(!this->curvature_grad.compute(max_error,max_iter,start_point,true))
- return std::numeric_limits<double>::infinity();
+ return std::numeric_limits<double>::max();
else
{
this->curvature_computed=true;
@@ -821,7 +821,7 @@ double CG2Spline::get_max_curvature_der(double max_error,unsigned int max_iter)
{
start_point=this->get_max_sample_point(boost::bind(&CG2Spline::curvature_der_pow2,this,_1));
if(!this->curvature_der_grad.compute(max_error,max_iter,start_point,true))
- return std::numeric_limits<double>::infinity();
+ return std::numeric_limits<double>::max();
else
{
this->curvature_der_computed=true;
@@ -839,7 +839,7 @@ double CG2Spline::find_closest_point(double x, double y, TPoint &point,double ma
this->dist_y=y;
start_point=this->get_min_sample_point(boost::bind(&CG2Spline::distance_pow2,this,_1));
if(!this->distance_grad.compute(max_error,max_iter,start_point,false))
- return std::numeric_limits<double>::infinity();
+ return std::numeric_limits<double>::max();
else
{
point=this->evaluate_parameter(this->distance_grad.get_coordinate());
@@ -858,7 +858,7 @@ double CG2Spline::find_closest_point(double x, double y,double max_error,unsigne
this->dist_y=y;
start_point=this->get_min_sample_point(boost::bind(&CG2Spline::distance_pow2,this,_1));
if(!this->distance_grad.compute(max_error,max_iter,start_point,false))
- return std::numeric_limits<double>::infinity();
+ return std::numeric_limits<double>::max();
else
{
if(this->num_points)
diff --git a/src/gradient.cpp b/src/gradient.cpp
index 92c876fe0fdbce9f1fc11d9a41bf2b9834d13756..3301bb41f45e15b9b240899469620be08f75360a 100644
--- a/src/gradient.cpp
+++ b/src/gradient.cpp
@@ -52,6 +52,7 @@ bool CGradient::compute(double max_func_error,unsigned int max_iter,double start
unsigned int iteration=0;
double gamma=1.0,func_value,func_der_value,prev_func_value,prev_func_der_value;
double coord,prev_coord,coord_inc;
+ bool beyond_limits=false;
coord=start_point;
func_value=this->p_function(coord);
@@ -72,6 +73,7 @@ bool CGradient::compute(double max_func_error,unsigned int max_iter,double start
coord=this->max_coord;
this->coordinate=coord;
this->value=this->p_function(coord);
+ beyond_limits=true;
break;
}
else if(coord<this->min_coord)
@@ -79,6 +81,7 @@ bool CGradient::compute(double max_func_error,unsigned int max_iter,double start
coord=this->min_coord;
this->coordinate=coord;
this->value=this->p_function(coord);
+ beyond_limits=true;
break;
}
prev_func_value=func_value;
@@ -90,7 +93,7 @@ bool CGradient::compute(double max_func_error,unsigned int max_iter,double start
}while((fabs(func_value-prev_func_value)>max_func_error || fabs(coord-prev_coord)>0.001) && iteration<max_iter);
this->coordinate=coord;
this->value=func_value;
- if(iteration==max_iter)
+ if(iteration==max_iter || beyond_limits)
return false;
else
return true;
diff --git a/src/opendrive_lane.cpp b/src/opendrive_lane.cpp
index 8e2f2457f7576ab19c264689e820071c715e8cc7..035100b03ed1c619ff82b26e11fc75173a9a1255 100644
--- a/src/opendrive_lane.cpp
+++ b/src/opendrive_lane.cpp
@@ -779,22 +779,25 @@ TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose
unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose found_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->nodes.size();i++)
{
- this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
- dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
+ dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ distance=length;
+ }
}
}
- distance=min_dist;
return closest_index;
}
@@ -810,7 +813,7 @@ const COpendriveRoadNode &COpendriveLane::get_closest_node(TOpendriveWorldPose &
double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max(),distance;
+ double dist,min_dist=std::numeric_limits<double>::max(),distance,length;
TOpendriveWorldPose found_pose;
unsigned int closest_index=-1;
COpendriveLink *link;
@@ -820,17 +823,20 @@ double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
closest_pose.heading=std::numeric_limits<double>::max();
for(unsigned int i=0;i<this->nodes.size();i++)
{
- this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
- dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
- closest_pose=found_pose;
+ dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ closest_pose=found_pose;
+ distance=length;
+ }
}
}
- distance=0.0;
for(unsigned int i=0;i<this->nodes.size();i++)
{
if(i<closest_index)
@@ -842,7 +848,6 @@ double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
else
break;
}
- distance+=min_dist;
return distance;
}
diff --git a/src/opendrive_road.cpp b/src/opendrive_road.cpp
index f26267a49ffd4fbff5286e72aeb48f07482500f0..42a9a77a2e4fbac46f958e6c1eab6ea52759e1ad 100644
--- a/src/opendrive_road.cpp
+++ b/src/opendrive_road.cpp
@@ -671,18 +671,22 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
unsigned int COpendriveRoad::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->nodes.size();i++)
{
- this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ distance=length;
+ }
}
}
@@ -701,18 +705,22 @@ const COpendriveRoadNode &COpendriveRoad::get_closest_node(TOpendriveWorldPose &
unsigned int COpendriveRoad::get_closest_lane_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->lanes.size();i++)
{
- this->lanes[i]->get_closest_pose(pose,closest_pose,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
- {
- min_dist=dist;
- closest_index=i;
+ length=this->lanes[i]->get_closest_pose(pose,closest_pose,angle_tol);
+ if(length<std::numeric_limits<double>::max())
+ {
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ distance=length;
+ }
}
}
@@ -731,18 +739,22 @@ const COpendriveLane &COpendriveRoad::get_closest_lane(TOpendriveWorldPose &pose
unsigned int COpendriveRoad::get_closest_segment_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->segments.size();i++)
{
- this->segments[i]->get_closest_pose(pose,closest_pose,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
- {
- min_dist=dist;
- closest_index=i;
+ length=this->segments[i]->get_closest_pose(pose,closest_pose,angle_tol);
+ if(length<std::numeric_limits<double>::max())
+ {
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ distance=length;
+ }
}
}
@@ -768,12 +780,15 @@ double COpendriveRoad::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorl
for(unsigned int i=0;i<this->nodes.size();i++)
{
length=this->nodes[i]->get_closest_pose(pose,pose_found,false,angle_tol);
- dist=sqrt(pow(pose_found.x-pose.x,2.0)+pow(pose_found.y-pose.y,2.0));
- if(dist<min_dist)
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_pose=pose_found;
- closest_length=length;
+ dist=sqrt(pow(pose_found.x-pose.x,2.0)+pow(pose_found.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_pose=pose_found;
+ closest_length=length;
+ }
}
}
@@ -784,16 +799,28 @@ void COpendriveRoad::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOp
{
std::vector<TOpendriveWorldPose> poses;
std::vector<double> lengths;
+ bool already_added;
closest_poses.clear();
closest_lengths.clear();
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_poses(pose,poses,lengths,dist_tol,false,angle_tol);
- for(unsigned int j=0;j<poses.size();i++)
+ for(unsigned int j=0;j<poses.size();j++)
{
- closest_poses.push_back(poses[i]);
- closest_lengths.push_back(lengths[i]);
+ already_added=false;
+ for(unsigned int k=0;k<closest_poses.size();k++)
+ if(fabs(closest_poses[k].x-poses[j].x)<this->resolution &&
+ fabs(closest_poses[k].y-poses[j].y)<this->resolution)
+ {
+ already_added=true;
+ break;
+ }
+ if(!already_added)
+ {
+ closest_poses.push_back(poses[j]);
+ closest_lengths.push_back(lengths[j]);
+ }
}
}
}
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index ee7d6dfaa6384c43acdfe4a013feb2119fb08c75..bff1134db2d95040609a762c56e1bd516b6b48b1 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -47,7 +47,7 @@ bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
double start_curvature,end_curvature;
COpendriveLink *new_link;
- if(this->has_link_with(node) || node->has_link_with(this))
+ if(this->has_link_with(*node) || node->has_link_with(*this))
return false;
new_link=new COpendriveLink();
new_link->set_prev(this);
@@ -118,10 +118,10 @@ bool COpendriveRoadNode::has_link(COpendriveLink *link)
return false;
}
-bool COpendriveRoadNode::has_link_with(COpendriveRoadNode *node)
+bool COpendriveRoadNode::has_link_with(const COpendriveRoadNode &node) const
{
for(unsigned int i=0;i<this->links.size();i++)
- if(this->links[i]->prev==this && this->links[i]->next==node)
+ if(this->links[i]->prev==this && this->links[i]->next==&node)
return true;
return false;
@@ -325,7 +325,6 @@ void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPos
{
std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
- TOpendriveRoadNodeParent *parent;
// remove the references to all lanes and segments except for lane
for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
@@ -333,10 +332,7 @@ void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPos
if(parent_it->lane!=lane)
parent_it=this->parents.erase(parent_it);
else
- {
- parent=&(*parent_it);
parent_it++;
- }
}
// update the links
for(link_it=this->links.begin();link_it!=this->links.end();)
@@ -438,22 +434,25 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,doub
{
double dist,min_dist=std::numeric_limits<double>::max();
unsigned int closest_index=-1;
- double angle;
+ double angle,length;
TOpendriveWorldPose closest_pose;
for(unsigned int i=0;i<this->links.size();i++)
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
- this->links[i]->find_closest_world_pose(pose,closest_pose);
- angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
- if(fabs(angle)<angle_tol)
+ length=this->links[i]->find_closest_world_pose(pose,closest_pose);
+ if(length<std::numeric_limits<double>::max())
{
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
+ if(fabs(angle)<angle_tol)
{
- min_dist=dist;
- closest_index=i;
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ }
}
}
}
@@ -465,7 +464,7 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,doub
double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,bool only_lanes,double angle_tol)const
{
double dist,min_dist=std::numeric_limits<double>::max();
- double angle;
+ double angle,length;
TOpendriveWorldPose closest_pose;
for(unsigned int i=0;i<this->links.size();i++)
@@ -474,13 +473,16 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,bool o
{
if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
- this->links[i]->find_closest_world_pose(pose,closest_pose);
- angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
- if(fabs(angle)<angle_tol)
+ length=this->links[i]->find_closest_world_pose(pose,closest_pose);
+ if(length<std::numeric_limits<double>::max())
{
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
- min_dist=dist;
+ angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
+ if(fabs(angle)<angle_tol)
+ {
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ min_dist=dist;
+ }
}
}
}
@@ -506,15 +508,18 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
length=this->links[i]->find_closest_world_pose(pose,tmp_pose);
- angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
- if(fabs(angle)<angle_tol)
+ if(length<std::numeric_limits<double>::max())
{
- dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
+ if(fabs(angle)<angle_tol)
{
- min_dist=dist;
- closest_length=length;
- closest_pose=tmp_pose;
+ dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_length=length;
+ closest_pose=tmp_pose;
+ }
}
}
}
@@ -530,6 +535,7 @@ void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector
double angle;
double length;
TOpendriveWorldPose closest_pose;
+ bool already_added;
closest_poses.clear();
closest_lengths.clear();
@@ -540,14 +546,28 @@ void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector
if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
length=this->links[i]->find_closest_world_pose(pose,closest_pose);
- angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
- if(fabs(angle)<angle_tol)
+ if(length<std::numeric_limits<double>::max())
{
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<=dist_tol)
+ angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
+ if(fabs(angle)<angle_tol)
{
- closest_poses.push_back(closest_pose);
- closest_lengths.push_back(length);
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<=dist_tol)
+ {
+ already_added=false;
+ for(unsigned int j=0;j<closest_poses.size();j++)
+ if(fabs(closest_poses[j].x-closest_pose.x)<this->resolution &&
+ fabs(closest_poses[j].y-closest_pose.y)<this->resolution)
+ {
+ already_added=true;
+ break;
+ }
+ if(!already_added)
+ {
+ closest_poses.push_back(closest_pose);
+ closest_lengths.push_back(length);
+ }
+ }
}
}
}
diff --git a/src/opendrive_road_segment.cpp b/src/opendrive_road_segment.cpp
index 60186394132d3276dc74e92e3a0646d11ee41195..c48d4765bccadfad4a915da4f8b0d61d767a144b 100644
--- a/src/opendrive_road_segment.cpp
+++ b/src/opendrive_road_segment.cpp
@@ -732,9 +732,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
{
length=geom_it->spline->find_closest_point(start_pose->x,start_pose->y,new_point);
- if(fabs(length-geom_it->spline->get_length())<this->resolution)
- geom_it=new_segment->geometries.erase(geom_it);
- else
+ if(length<std::numeric_limits<double>::max())
{
geom_it->spline->set_start_point(new_point);
geom_it->spline->generate(this->resolution);
@@ -742,6 +740,8 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
geom_it->opendrive->set_max_s(geom_it->opendrive->get_max_s()-length);
break;
}
+ else
+ geom_it=new_segment->geometries.erase(geom_it);
}
for(unsigned int i=1;i<new_segment->geometries.size();i++)
{
@@ -754,7 +754,7 @@ COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new
for(geom_it=new_segment->geometries.begin();geom_it!=new_segment->geometries.end();)
{
length=geom_it->spline->find_closest_point(end_pose->x,end_pose->y,new_point);
- if(length<geom_it->spline->get_length())
+ if(length<std::numeric_limits<double>::max())
{
geom_it->spline->set_end_point(new_point);
geom_it->spline->generate(this->resolution);
@@ -1150,18 +1150,22 @@ TOpendriveWorldPose COpendriveRoadSegment::transform_to_world_pose(const TOpendr
unsigned int COpendriveRoadSegment::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
unsigned int closest_index=-1;
for(unsigned int i=0;i<this->nodes.size();i++)
{
- this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ length=this->nodes[i]->get_closest_pose(pose,closest_pose,false,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ distance=length;
+ }
}
}
@@ -1180,19 +1184,23 @@ const COpendriveRoadNode &COpendriveRoadSegment::get_closest_node(TOpendriveWorl
int COpendriveRoadSegment::get_closest_lane_id(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max();
+ double dist,min_dist=std::numeric_limits<double>::max(),length;
TOpendriveWorldPose closest_pose;
int closest_id=0;
std::map<int,COpendriveLane *>::const_iterator lane_it;
for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
{
- lane_it->second->get_closest_pose(pose,closest_pose,angle_tol);
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ length=lane_it->second->get_closest_pose(pose,closest_pose,angle_tol);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_id=lane_it->first;
+ dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_id=lane_it->first;
+ distance=length;
+ }
}
}
@@ -1209,7 +1217,7 @@ const COpendriveLane &COpendriveRoadSegment::get_closest_lane(TOpendriveWorldPos
double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
- double dist,min_dist=std::numeric_limits<double>::max(),distance;
+ double dist,min_dist=std::numeric_limits<double>::max(),distance,length;
unsigned int closest_index=-1;
TPoint closest_spline_point;
@@ -1218,22 +1226,30 @@ double COpendriveRoadSegment::get_closest_pose(TOpendriveWorldPose &pose,TOpendr
closest_pose.heading=std::numeric_limits<double>::max();
for(unsigned int i=0;i<this->geometries.size();i++)
{
- this->geometries[i].spline->find_closest_point(pose.x,pose.y,closest_spline_point);
- dist=sqrt(pow(closest_spline_point.x-pose.x,2.0)+pow(closest_spline_point.y-pose.y,2.0));
- if(dist<min_dist)
+ length=this->geometries[i].spline->find_closest_point(pose.x,pose.y,closest_spline_point);
+ if(length<std::numeric_limits<double>::max())
{
- min_dist=dist;
- closest_index=i;
- closest_pose.x=closest_spline_point.x;
- closest_pose.y=closest_spline_point.y;
- closest_pose.heading=normalize_angle(closest_spline_point.heading);
+ dist=sqrt(pow(closest_spline_point.x-pose.x,2.0)+pow(closest_spline_point.y-pose.y,2.0));
+ if(dist<min_dist)
+ {
+ min_dist=dist;
+ closest_index=i;
+ closest_pose.x=closest_spline_point.x;
+ closest_pose.y=closest_spline_point.y;
+ closest_pose.heading=normalize_angle(closest_spline_point.heading);
+ distance=length;
+ }
}
}
- distance=0.0;
- for(unsigned int i=0;i<closest_index;i++)
- distance+=this->geometries[i].spline->get_length();
- distance+=min_dist;
+ if(closest_index!=(unsigned int)-1)
+ {
+ for(unsigned int i=0;i<closest_index;i++)
+ distance+=this->geometries[i].spline->get_length();
+ distance+=min_dist;
+ }
+ else
+ distance=std::numeric_limits<double>::max();
return distance;
}