Commit 6676e006 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a reference to the segment and lane to which the link belongs.

Added functions to update the start and end positions of the link.
Used a single update_reference() function with all the new segment,lane and node references.
parent 7146700e
#ifndef _OPENDRIVE_LINK_H
#define _OPENDRIVE_LINK_H
#include "opendrive_common.h"
#include "g2_spline.h"
#include "opendrive_line.h"
#include "opendrive_spiral.h"
#include "opendrive_arc.h"
#include "opendrive_param_poly3.h"
class COpendriveRoadNode;
#include "opendrive_road_node.h"
class COpendriveLink
{
friend class COpendriveRoadSegment;
friend class COpendriveRoadNode;
friend class COpendriveRoad;
private:
COpendriveRoadNode *prev;
COpendriveRoadNode *next;
COpendriveRoadSegment *segment;
COpendriveLane *lane;
CG2Spline *spline;
opendrive_mark_t mark;
double resolution;
......@@ -25,14 +29,21 @@ class COpendriveLink
void set_prev(COpendriveRoadNode *node);
void set_next(COpendriveRoadNode *node);
void set_road_mark(opendrive_mark_t mark);
void set_parent_segment(COpendriveRoadSegment *segment);
void set_parent_lane(COpendriveLane *lane);
void set_resolution(double res);
void set_scale_factor(double scale);
void generate(double start_curvature,double end_curvature,double length,bool lane);
void update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs);
void update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lanei_refs,node_up_ref_t &node_refs);
bool clean_references(node_up_ref_t &refs);
void update_start_pose(TOpendriveWorldPoint *start=NULL);
void update_end_pose(TOpendriveWorldPoint *end=NULL);
public:
const COpendriveRoadNode &get_prev(void) const;
const COpendriveRoadNode &get_next(void) const;
opendrive_mark_t get_road_mark(void) const;
const COpendriveRoadSegment &get_parent_segment(void) const;
const COpendriveLane &get_parent_lane(void) const;
double get_resolution(void) const;
double get_scale_factor(void) const;
double find_closest_world_point(TOpendriveWorldPoint &world,TPoint &point);
......
......@@ -5,6 +5,8 @@ COpendriveLink::COpendriveLink()
{
this->prev=NULL;
this->next=NULL;
this->segment=NULL;
this->lane=NULL;
this->spline=NULL;
this->mark=OD_MARK_NONE;
this->resolution=DEFAULT_RESOLUTION;
......@@ -15,6 +17,8 @@ COpendriveLink::COpendriveLink(const COpendriveLink &object)
{
this->prev=object.prev;
this->next=object.next;
this->segment=object.segment;
this->lane=object.lane;
this->spline=new CG2Spline(*object.spline);
this->mark=object.mark;
this->resolution=object.resolution;
......@@ -36,6 +40,16 @@ void COpendriveLink::set_road_mark(opendrive_mark_t mark)
this->mark=mark;
}
void COpendriveLink::set_parent_segment(COpendriveRoadSegment *segment)
{
this->segment=segment;
}
void COpendriveLink::set_parent_lane(COpendriveLane *lane)
{
this->lane=lane;
}
void COpendriveLink::set_resolution(double res)
{
this->resolution=res;
......@@ -65,10 +79,55 @@ void COpendriveLink::generate(double start_curvature,double end_curvature,double
this->spline->generate(this->resolution,length);
}
void COpendriveLink::update_references(std::map<COpendriveRoadNode *,COpendriveRoadNode *> refs)
void COpendriveLink::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
if(node_refs.find(this->prev)!=node_refs.end())
this->prev=node_refs[this->prev];
if(node_refs.find(this->next)!=node_refs.end())
this->next=node_refs[this->next];
if(segment_refs.find(this->segment)!=segment_refs.end())
this->segment=segment_refs[this->segment];
if(lane_refs.find(this->lane)!=lane_refs.end())
this->lane=lane_refs[this->lane];
}
bool COpendriveLink::clean_references(node_up_ref_t &refs)
{
if(refs.find(this->prev)!=refs.end())
this->prev=refs[this->prev];
else if(!this->prev->updated(refs))
return true;
if(refs.find(this->next)!=refs.end())
this->next=refs[this->next];
else if(!this->next->updated(refs))
return true;
return false;
}
void COpendriveLink::update_start_pose(TOpendriveWorldPoint *start)
{
TPoint spline_start;
double length;
if(start==NULL)
return;
length=this->find_closest_world_point(*start,spline_start);
length=this->spline->get_length()-length;
this->spline->set_start_point(spline_start);
this->spline->generate(this->resolution,length);
}
void COpendriveLink::update_end_pose(TOpendriveWorldPoint *end)
{
this->prev=refs[this->prev];
this->next=refs[this->next];
TPoint spline_end;
double length;
if(end==NULL)
return;
length=this->find_closest_world_point(*end,spline_end);
this->spline->set_end_point(spline_end);
this->spline->generate(this->resolution,length);
}
const COpendriveRoadNode &COpendriveLink::get_prev(void) const
......@@ -86,6 +145,16 @@ opendrive_mark_t COpendriveLink::get_road_mark(void) const
return this->mark;
}
const COpendriveRoadSegment &COpendriveLink::get_parent_segment(void) const
{
return *this->segment;
}
const COpendriveLane &COpendriveLink::get_parent_lane(void) const
{
return *this->lane;
}
double COpendriveLink::get_resolution(void) const
{
return this->resolution;
......@@ -101,7 +170,10 @@ double COpendriveLink::find_closest_world_point(TOpendriveWorldPoint &world,TPoi
double length;
if(this->spline!=NULL)
{
length=this->spline->find_closest_point(world.x,world.y,point);
point.heading=normalize_angle(point.heading);
}
else
length=std::numeric_limits<double>::max();
......@@ -160,8 +232,8 @@ double COpendriveLink::get_length(void) const
std::ostream& operator<<(std::ostream& out, COpendriveLink &link)
{
out << " Previous node: " << link.get_prev().get_index() << " of road " << link.get_prev().get_parent_segment().get_name() << std::endl;
out << " Next node: " << link.get_next().get_index() << " of road " << link.get_next().get_parent_segment().get_name() << std::endl;
out << " Previous node: " << link.get_prev().get_index() << std::endl;
out << " Next node: " << link.get_next().get_index() << std::endl;
out << " Road mark: ";
switch(link.get_road_mark())
{
......@@ -195,6 +267,8 @@ COpendriveLink::~COpendriveLink()
{
this->prev=NULL;
this->next=NULL;
this->segment=NULL;
this->lane=NULL;
if(this->spline!=NULL)
{
delete this->spline;
......
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