Commit d2f67dbe authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Modified the gradient class to return false when the gradient goes outside the limits.

The spline class returns maximum length when the is no closest point belonging to the spline.
All other classes check the returned length to check for the existance of the closest point.
parent 41858975
......@@ -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();
};
......
......@@ -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)
......
......@@ -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;
......
......@@ -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;
}
......
......@@ -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]);
}
}
}
}
......
......@@ -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);
}
}
}
}
}
......
......@@ -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;</