Commit 3c988839 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Function to find the closest point in the road operational.

parent 59304e44
......@@ -42,7 +42,7 @@ class COpendriveRoad
unsigned int get_num_nodes(void) const;
const COpendriveRoadNode& get_node(unsigned int index) const;
const COpendriveRoadSegment& operator[](std::size_t index);
double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point);
double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
void operator=(const COpendriveRoad& object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
~COpendriveRoad();
......
......@@ -48,7 +48,7 @@ class COpendriveRoadNode
const COpendriveLane &get_lane(unsigned int index) const;
unsigned int get_num_geometries(void) const;
const COpendriveGeometry &get_geometry(unsigned int index) const;
double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point);
double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
};
......
......@@ -316,23 +316,23 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
throw CException(_HERE_,"Invalid segment index");
}
double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point)
double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
{
double dist,min_dist=std::numeric_limits<double>::max();
double angle,min_angle=std::numeric_limits<double>::max();
double angle;
TOpendriveWorldPoint point;
double length,closest_length;
for(unsigned int i=0;i<this->nodes.size();i++)
{
length=this->nodes[i]->get_closest_point(x,y,heading,point);
length=this->nodes[i]->get_closest_point(x,y,heading,point,angle_tol);
dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
std::cout << point.x << "," << point.y << "," << point.heading << "," << length << "," << dist << std::endl;
angle=diff_angle(normalize_angle(heading),point.heading);
if(angle<min_angle)
if(fabs(angle)<angle_tol)
{
dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
if(dist<min_dist)
{
min_angle=angle;
min_dist=dist;
closest_point=point;
closest_length=length;
......
......@@ -275,10 +275,10 @@ const COpendriveGeometry &COpendriveRoadNode::get_geometry(unsigned int index) c
throw CException(_HERE_,"Invalid geometry index");
}
double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point)
double COpendriveRoadNode::get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol)
{
double dist,min_dist=std::numeric_limits<double>::max();
double angle,min_angle=std::numeric_limits<double>::max();
double angle;
double length,closest_length;
TOpendriveWorldPoint target;
TPoint point;
......@@ -291,13 +291,13 @@ double COpendriveRoadNode::get_closest_point(double x, double y,double heading,T
if(&this->links[i]->get_prev()==this)// links starting at this node
{
length=this->links[i]->find_closest_world_point(target,point);
std::cout << " " << point.x << "," << point.y << "," << point.heading << "," << length << std::endl;
angle=diff_angle(normalize_angle(heading),point.heading);
if(angle<min_angle)
if(fabs(angle)<angle_tol)
{
dist=sqrt(pow(point.x-x,2.0)+pow(point.y-y,2.0));
if(dist<min_dist)
{
min_angle=angle;
min_dist=dist;
closest_point.x=point.x;
closest_point.y=point.y;
......
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