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

Added some functions:

  * A function to get the closes node to a point.
  * A function to get the set of closest points to a point.
parent 19d30ab3
......@@ -2,7 +2,7 @@
#define _OPENDRIVE_ROAD_H
#ifdef _HAVE_XSD
#include "../src/xml/OpenDRIVE_1.4H.hxx"
#include "xml/OpenDRIVE_1.4H.hxx"
#endif
#include "opendrive_road_segment.h"
......@@ -42,7 +42,9 @@ class COpendriveRoad
unsigned int get_num_nodes(void) const;
const COpendriveRoadNode& get_node(unsigned int index) const;
const COpendriveRoadSegment& operator[](std::size_t index);
unsigned int get_closest_node(double x, double y,double heading,double angle_tol=0.1);
double get_closest_point(double x, double y,double heading,TOpendriveWorldPoint &closest_point,double angle_tol=0.1);
void get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol=0.1);
void operator=(const COpendriveRoad& object);
friend std::ostream& operator<<(std::ostream& out, COpendriveRoad &road);
~COpendriveRoad();
......
......@@ -316,10 +316,30 @@ const COpendriveRoadSegment& COpendriveRoad::operator[](std::size_t index)
throw CException(_HERE_,"Invalid segment index");
}
unsigned int COpendriveRoad::get_closest_node(double x, double y,double heading,double angle_tol)
{
double dist,min_dist=std::numeric_limits<double>::max();
TOpendriveWorldPoint point;
unsigned int closest_index;
for(unsigned int i=0;i<this->nodes.size();i++)
{
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));
if(dist<min_dist)
{
min_dist=dist;
closest_index=i;
}
}
return closest_index;
}
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;
TOpendriveWorldPoint point;
double length,closest_length;
......@@ -327,22 +347,35 @@ double COpendriveRoad::get_closest_point(double x, double y,double heading,TOpen
{
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(fabs(angle)<angle_tol)
if(dist<min_dist)
{
if(dist<min_dist)
{
min_dist=dist;
closest_point=point;
closest_length=length;
}
min_dist=dist;
closest_point=point;
closest_length=length;
}
}
return closest_length;
}
void COpendriveRoad::get_closest_points(double x, double y,double heading,std::vector<TOpendriveWorldPoint> &closest_points,std::vector<double> &closest_lengths,double dist_tol,double angle_tol)
{
std::vector<TOpendriveWorldPoint> points;
std::vector<double> lengths;
closest_points.clear();
closest_lengths.clear();
for(unsigned int i=0;i<this->nodes.size();i++)
{
this->nodes[i]->get_closest_points(x,y,heading,points,lengths,dist_tol,angle_tol);
for(unsigned int j=0;j<points.size();i++)
{
closest_points.push_back(points[i]);
closest_lengths.push_back(lengths[i]);
}
}
}
void COpendriveRoad::operator=(const COpendriveRoad& object)
{
COpendriveRoadSegment *segment;
......
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