diff --git a/include/opendrive_road_node.h b/include/opendrive_road_node.h
index 49fa7a6742a5a9b98fde459b146d9da3a817b1e7..c5625b06fb8c79f91eed62c1bb234183d622d70a 100644
--- a/include/opendrive_road_node.h
+++ b/include/opendrive_road_node.h
@@ -13,7 +13,6 @@ typedef struct
COpendriveRoadSegment *segment;
double start_curvature;
double end_curvature;
- double length;
}TOpendriveRoadNodeParent;
class COpendriveRoadNode
@@ -31,7 +30,7 @@ class COpendriveRoadNode
unsigned int index;
protected:
COpendriveRoadNode();
- COpendriveRoadNode(const COpendriveRoadNode &object);
+ COpendriveRoadNode *clone(link_up_ref_t &new_link_ref);
void free(void);
bool add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane);
void add_link(COpendriveLink *link);
@@ -43,15 +42,15 @@ class COpendriveRoadNode
void set_scale_factor(double scale);
void set_index(unsigned int index);
void set_pose(TOpendriveWorldPose &start);
- void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double length,double start_curvature,double end_curvature);
+ void add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature);
TOpendriveRoadNodeParent *get_parent_with_lane(const COpendriveLane *lane);
TOpendriveRoadNodeParent *get_parent_with_segment(const COpendriveRoadSegment *segment);
bool updated(node_up_ref_t &refs);
COpendriveRoadNode *get_original_node(node_up_ref_t &node_refs);
void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
void clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs);
- void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose *start=NULL);
- void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose *end=NULL);
+ void update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance);
+ void update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance);
public:
double get_resolution(void) const;
double get_scale_factor(void) const;
@@ -64,9 +63,9 @@ class COpendriveRoadNode
const COpendriveLink &get_link(unsigned int index) const;
const COpendriveLink &get_link_ending_at(unsigned int node_index) const;
unsigned int get_closest_link(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
- double get_closest_distance(TOpendriveWorldPose &pose,double angle_tol=0.1) const;
- double get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,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,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;
friend std::ostream& operator<<(std::ostream& out, COpendriveRoadNode &node);
~COpendriveRoadNode();
};
diff --git a/src/opendrive_road_node.cpp b/src/opendrive_road_node.cpp
index f9a990f36d7198f643daae266a22adf02489bb81..ee7d6dfaa6384c43acdfe4a013feb2119fb08c75 100644
--- a/src/opendrive_road_node.cpp
+++ b/src/opendrive_road_node.cpp
@@ -15,29 +15,37 @@ COpendriveRoadNode::COpendriveRoadNode()
this->index=-1;
}
-COpendriveRoadNode::COpendriveRoadNode(const COpendriveRoadNode &object)
+COpendriveRoadNode *COpendriveRoadNode::clone(link_up_ref_t &new_link_ref)
{
- COpendriveLink *new_link;
+ COpendriveRoadNode *new_node;
- this->resolution=object.resolution;
- this->scale_factor=object.scale_factor;
- this->pose=object.pose;
- this->parents=object.parents;
- this->links.clear();
- for(unsigned int i=0;i<object.links.size();i++)
+ new_node=new COpendriveRoadNode();
+ new_node->resolution=this->resolution;
+ new_node->scale_factor=this->scale_factor;
+ new_node->pose=this->pose;
+ new_node->parents=this->parents;
+ new_node->links.resize(this->links.size());
+ for(unsigned int i=0;i<this->links.size();i++)
{
- new_link=new COpendriveLink(*object.links[i]);
- this->links.push_back(new_link);
+ if(new_link_ref.find(this->links[i])!=new_link_ref.end())
+ new_node->links[i]=new_link_ref[this->links[i]];
+ else
+ {
+ new_node->links[i]=new COpendriveLink(*this->links[i]);
+ new_link_ref[this->links[i]]=new_node->links[i];
+ }
}
- this->index=object.index;
+ new_node->index=this->index;
+
+ return new_node;
}
bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark,COpendriveRoadSegment *segment,COpendriveLane *lane)
{
+ TOpendriveRoadNodeParent *parent;
TOpendriveWorldPose node_pose;
- double start_curvature,end_curvature,length;
+ double start_curvature,end_curvature;
COpendriveLink *new_link;
- bool lane_found=false;
if(this->has_link_with(node) || node->has_link_with(this))
return false;
@@ -51,24 +59,26 @@ bool COpendriveRoadNode::add_link(COpendriveRoadNode *node,opendrive_mark_t mark
new_link->set_parent_lane(lane);
// get the curvature
node_pose=node->get_pose();
- for(unsigned int i=0;i<this->parents.size();i++)
+ parent=this->get_parent_with_lane(lane);
+ if(parent!=NULL)
{
- if(this->parents[i].lane==lane && this->parents[i].segment==segment)
- {
- start_curvature=this->parents[i].start_curvature;
- end_curvature=this->parents[i].end_curvature;
- length=this->parents[i].length;
- lane_found=true;
- break;
- }
+ start_curvature=parent->start_curvature;
+ end_curvature=parent->end_curvature;
}
- if(!lane_found)
+ else
{
- start_curvature=0.0;
- end_curvature=0.0;
- length=sqrt(pow(this->pose.x-node_pose.x,2.0)+pow(this->pose.y-node_pose.y,2.0));
+ if(this->get_num_parents()==1)
+ {
+ start_curvature=this->parents[0].start_curvature;
+ end_curvature=this->parents[0].end_curvature;
+ }
+ else
+ {
+ start_curvature=0.0;
+ end_curvature=0.0;
+ }
}
- new_link->generate(start_curvature,end_curvature,length,lane_found);
+ new_link->generate(start_curvature,end_curvature);
this->add_link(new_link);
node->add_link(new_link);
@@ -89,7 +99,8 @@ void COpendriveRoadNode::remove_link(COpendriveLink *link)
{
if((*it)==link)
{
- delete (*it);
+ if((*it)->prev==this)
+ delete (*it);
it=this->links.erase(it);
break;
}
@@ -146,7 +157,6 @@ void COpendriveRoadNode::set_scale_factor(double scale)
{
this->parents[i].start_curvature*=scale/this->scale_factor;
this->parents[i].end_curvature*=scale/this->scale_factor;
- this->parents[i].length*=this->scale_factor/scale;
}
}
@@ -160,7 +170,7 @@ void COpendriveRoadNode::set_pose(TOpendriveWorldPose &start)
this->pose=start;
}
-void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double length,double start_curvature,double end_curvature)
+void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *segment,double start_curvature,double end_curvature)
{
TOpendriveRoadNodeParent new_parent;
@@ -171,7 +181,6 @@ void COpendriveRoadNode::add_parent(COpendriveLane *lane,COpendriveRoadSegment *
new_parent.segment=segment;
new_parent.start_curvature=start_curvature;
new_parent.end_curvature=end_curvature;
- new_parent.length=length;
this->parents.push_back(new_parent);
}
@@ -272,15 +281,24 @@ void COpendriveRoadNode::clean_references(segment_up_ref_t &segment_refs,lane_up
}
}
-void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose *start)
+void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldPose &start,double distance)
{
std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
std::vector<COpendriveLink *>::iterator link_it;
- double new_length;
- TPoint new_pose;
+ TOpendriveRoadNodeParent *parent;
- if(start==NULL)
- return;
+ this->pose=start;
+ // remove the references to all lanes and segments except for lane
+ for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
+ {
+ 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();)
{
@@ -291,35 +309,35 @@ void COpendriveRoadNode::update_start_pose(COpendriveLane *lane,TOpendriveWorldP
}
else
{
- new_pose=(*link_it)->update_start_pose(start);
- new_length=(*link_it)->get_length();
+ if((*link_it)->lane==lane)
+ {
+ parent->start_curvature=(*link_it)->get_curvature_at(distance);
+ (*link_it)->update_start_pose(start,parent->start_curvature);
+ }
+ else
+ (*link_it)->update_start_pose(start,0.0);
link_it++;
}
}
+}
+
+void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose &end,double distance)
+{
+ 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();)
- {
+ {
if(parent_it->lane!=lane)
parent_it=this->parents.erase(parent_it);
else
{
+ parent=&(*parent_it);
parent_it++;
- parent_it->length=new_length;
- parent_it->start_curvature=new_pose.curvature;
}
}
-
-}
-
-void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPose *end)
-{
- std::vector<TOpendriveRoadNodeParent>::iterator parent_it;
- std::vector<COpendriveLink *>::iterator link_it;
- double new_length;
- TPoint new_pose;
-
- if(end==NULL)
- return;
// update the links
for(link_it=this->links.begin();link_it!=this->links.end();)
{
@@ -329,23 +347,7 @@ void COpendriveRoadNode::update_end_pose(COpendriveLane *lane,TOpendriveWorldPos
link_it=this->links.erase(link_it);
}
else
- {
- new_pose=(*link_it)->update_end_pose(end);
- new_length=(*link_it)->get_length();
link_it++;
- }
- }
- // remove the references to all lanes and segments except for lane
- for(parent_it=this->parents.begin();parent_it!=this->parents.end();)
- {
- if(parent_it->lane!=lane)
- parent_it=this->parents.erase(parent_it);
- else
- {
- parent_it++;
- parent_it->length=new_length;
- parent_it->end_curvature=new_pose.curvature;
- }
}
}
@@ -460,7 +462,7 @@ unsigned int COpendriveRoadNode::get_closest_link(TOpendriveWorldPose &pose,doub
return closest_index;
}
-double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,double angle_tol)const
+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;
@@ -470,13 +472,16 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,double
{
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)
+ if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
- 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;
+ 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)
+ {
+ 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;
+ }
}
}
}
@@ -484,7 +489,7 @@ double COpendriveRoadNode::get_closest_distance(TOpendriveWorldPose &pose,double
return min_dist;
}
-double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
+double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,bool only_lanes,double angle_tol) const
{
double dist,min_dist=std::numeric_limits<double>::max();
double angle;
@@ -498,16 +503,19 @@ 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,tmp_pose);
- angle=diff_angle(normalize_angle(pose.heading),tmp_pose.heading);
- if(fabs(angle)<angle_tol)
+ if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
- dist=sqrt(pow(tmp_pose.x-pose.x,2.0)+pow(tmp_pose.y-pose.y,2.0));
- if(dist<min_dist)
+ 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)
{
- 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;
+ }
}
}
}
@@ -516,7 +524,7 @@ double COpendriveRoadNode::get_closest_pose(TOpendriveWorldPose &pose,TOpendrive
return closest_length;
}
-void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,double angle_tol) const
+void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector<TOpendriveWorldPose> &closest_poses,std::vector<double> &closest_lengths,double dist_tol,bool only_lanes,double angle_tol) const
{
double dist;
double angle;
@@ -529,15 +537,18 @@ void COpendriveRoadNode::get_closest_poses(TOpendriveWorldPose &pose,std::vector
{
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);
- if(fabs(angle)<angle_tol)
+ if(!only_lanes || (only_lanes && this->links[i]->lane!=NULL))
{
- dist=sqrt(pow(closest_pose.x-pose.x,2.0)+pow(closest_pose.y-pose.y,2.0));
- if(dist<=dist_tol)
+ 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)
{
- 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)
+ {
+ closest_poses.push_back(closest_pose);
+ closest_lengths.push_back(length);
+ }
}
}
}