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

Solved a bug when computing the closest point: a valid pose was returned even if it was not found.

parent 00cf70ad
......@@ -596,6 +596,7 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
double dist,min_dist=std::numeric_limits<double>::max();
double angle;
double length,closest_length=std::numeric_limits<double>::max();
TOpendriveWorldPose tmp_pose;
closest_pose.x=std::numeric_limits<double>::max();
closest_pose.y=std::numeric_limits<double>::max();
......@@ -604,15 +605,16 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
{
if(&this->links[i]->get_prev()==this)// links starting at this node
{
length=this->links[i]->find_closest_world_pose(pose,closest_pose);
angle=diff_angle(normalize_angle(pose.heading),closest_pose.heading);
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)
{
dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
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;
}
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment