opendrive_road_segment.cpp

#include "opendrive_road_segment.h"
#include "exceptions.h"
#include <math.h>

COpendriveRoadSegment::COpendriveRoadSegment()
{
  this->name="";
  this->id=-1;
  this->lanes.clear();
  this->num_left_lanes=0;
  this->num_right_lanes=0;
  this->nodes.clear();
  this->signals.clear();
  this->objects.clear();
  this->connecting.clear();
  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
  this->resolution=DEFAULT_RESOLUTION;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
}

COpendriveRoadSegment::COpendriveRoadSegment(const COpendriveRoadSegment &object)
{
  COpendriveSignal *new_signal;
  COpendriveObject *new_object;

  this->name=object.name;
  this->id=object.id;
  this->lanes=object.lanes;
  this->num_right_lanes=object.num_right_lanes;
  this->num_left_lanes=object.num_left_lanes;
  this->nodes=object.nodes;
  this->parent_road=object.parent_road;
  this->signals.clear();
  for(unsigned int i=0;i<object.signals.size();i++)
  {
    new_signal=new COpendriveSignal(*object.signals[i]);
    this->signals.push_back(new_signal);
  }
  this->objects.clear();
  for(unsigned int i=0;i<object.objects.size();i++)
  {
    new_object=new COpendriveObject(*object.objects[i]);
    this->objects.push_back(new_object);
  }
  this->connecting=object.connecting;
  this->resolution=object.resolution;
  this->scale_factor=object.scale_factor;
  this->min_road_length=object.min_road_length;
  this->center_mark=object.center_mark;
}

void COpendriveRoadSegment::free(void)
{
  this->lanes.clear();
  this->num_left_lanes=0;
  this->num_right_lanes=0;
  for(unsigned int i=0;i<this->signals.size();i++)
  {
    delete this->signals[i];
    this->signals[i]=NULL;
  }
  this->nodes.clear();
  this->signals.clear();
  for(unsigned int i=0;i<this->objects.size();i++)
  {
    delete this->objects[i];
    this->objects[i]=NULL;
  }
  this->objects.clear();
  this->connecting.clear();
}

void COpendriveRoadSegment::set_resolution(double res)
{
  std::map<int,COpendriveLane *>::const_iterator lane_it;

  this->resolution=res;

  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
    lane_it->second->set_resolution(res);
}

void COpendriveRoadSegment::set_scale_factor(double scale)
{
  std::map<int,COpendriveLane *>::const_iterator lane_it;

  this->scale_factor=scale;

  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
    lane_it->second->set_scale_factor(scale);
  for(unsigned int i=0;i<this->signals.size();i++)
    this->signals[i]->set_scale_factor(scale);
  for(unsigned int i=0;i<this->objects.size();i++)
    this->objects[i]->set_scale_factor(scale);
}

void COpendriveRoadSegment::set_min_road_length(double length)
{
  this->min_road_length=length;
}

void COpendriveRoadSegment::set_parent_road(COpendriveRoad *parent)
{
  this->parent_road=parent;
}

void COpendriveRoadSegment::set_name(std::string &name)
{
  this->name=name;
}

void COpendriveRoadSegment::set_id(unsigned int id)
{
  this->id=id;
}

void COpendriveRoadSegment::set_center_mark(opendrive_mark_t mark)
{
  this->center_mark=mark;
}

bool COpendriveRoadSegment::updated(segment_up_ref_t &refs)
{
  segment_up_ref_t::iterator updated_it;

  for(updated_it=refs.begin();updated_it!=refs.end();updated_it++)
    if(updated_it->second==this)
      return true;

  return false;
}

void COpendriveRoadSegment::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
  std::vector<COpendriveRoadSegment *>::iterator seg_it;
  std::map<int,COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveRoadNode *>::iterator node_it;

  if(this->updated(segment_refs))
  {
    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();lane_it++)
    {
      if(lane_refs.find((*lane_it).second)!=lane_refs.end())
      {
        lane_it->second=lane_refs[lane_it->second];
        lane_it->second->update_references(segment_refs,lane_refs,node_refs);
      }
      else if(lane_it->second->updated(lane_refs))
        lane_it->second->update_references(segment_refs,lane_refs,node_refs);
    }
    for(node_it=this->nodes.begin();node_it!=this->nodes.end();node_it++)
      if(node_refs.find(*node_it)!=node_refs.end())
        (*node_it)=node_refs[*node_it];
    for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();seg_it++)
      if(segment_refs.find(*seg_it)!=segment_refs.end())
        (*seg_it)=segment_refs[*seg_it];
    for(unsigned int i=0;i<this->signals.size();i++)
      this->signals[i]->update_references(segment_refs);
    for(unsigned int i=0;i<this->objects.size();i++)
      this->objects[i]->update_references(segment_refs);
  }
}

void COpendriveRoadSegment::clean_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
  std::vector<COpendriveRoadSegment *>::iterator seg_it;
  std::map<int,COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveRoadNode *>::iterator node_it;

  if(this->updated(segment_refs))
  {
    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
    {
      if(lane_refs.find((*lane_it).second)!=lane_refs.end())
      {
        lane_it->second=lane_refs[lane_it->second];
        lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
        lane_it++;
      }
      else if(lane_it->second->updated(lane_refs))
      {
        lane_it->second->clean_references(segment_refs,lane_refs,node_refs);
        lane_it++;
      }
      else
        lane_it=this->lanes.erase(lane_it);
    }
    for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
    {
      if(node_refs.find(*node_it)!=node_refs.end())
      {
        (*node_it)=node_refs[*node_it];
        node_it++;
      }
      else if(!(*node_it)->updated(node_refs))
        node_it=this->nodes.erase(node_it);
      else
        node_it++;
    }
    for(seg_it=this->connecting.begin();seg_it!=this->connecting.end();)
    {
      if(segment_refs.find(*seg_it)!=segment_refs.end())
      {
        (*seg_it)=segment_refs[*seg_it];
        seg_it++;
      }
      else if(!(*seg_it)->updated(segment_refs))
        seg_it=this->connecting.erase(seg_it);
      else
        seg_it++;
    }
  }
}

void COpendriveRoadSegment::add_lanes(lanes::laneSection_type &lane_section)
{
  right::lane_iterator r_lane_it;
  left::lane_iterator l_lane_it;
  COpendriveLane *new_lane;
  std::stringstream text;
  unsigned int lane_index;

  // right lanes
  if(lane_section.right().present())
  {
    for(r_lane_it=lane_section.right()->lane().begin();r_lane_it!=lane_section.right()->lane().end();r_lane_it++)
    {
      try{
        new_lane=new COpendriveLane();
        new_lane->set_resolution(this->resolution);
        new_lane->set_scale_factor(this->scale_factor);
        new_lane->load(*r_lane_it,this);
        this->lanes[new_lane->get_id()]=new_lane;
        lane_index=parent_road->add_lane(new_lane);
        new_lane->set_index(lane_index);
        this->num_right_lanes++;
      }catch(CException &e){
        text << e.what() << " in road " << this->name;
        throw CException(_HERE_,text.str());
      }
    }
  }
  // left lanes
  if(lane_section.left().present())
  {
    for(l_lane_it=lane_section.left()->lane().begin();l_lane_it!=lane_section.left()->lane().end();l_lane_it++)
    {
      try{
        new_lane=new COpendriveLane();
        new_lane->set_resolution(this->resolution);
        new_lane->set_scale_factor(this->scale_factor);
        new_lane->load(*l_lane_it,this);
        this->lanes[new_lane->get_id()]=new_lane;
        lane_index=parent_road->add_lane(new_lane);
        new_lane->set_index(lane_index);
        this->num_left_lanes++;
      }catch(CException &e){
        text << e.what() << " in road " << this->name;
        throw CException(_HERE_,text.str());
      }
    }
  }
}

void COpendriveRoadSegment::remove_lane(COpendriveLane *lane)
{
  std::map<int,COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveLane *>::iterator other_lane_it;

  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();)
  {
    if(lane_it->second==lane)
    {
      if(lane_it->first>0)
        this->num_left_lanes--;
      else
        this->num_right_lanes--;
      // remove associated nodes
      for(unsigned int i=0;i<lane->nodes.size();i++)
        this->remove_node(lane->nodes[i]);
      // remove the reference from neightbour lanes
      if(lane_it->second->left_lane!=NULL)
        lane_it->second->left_lane->right_lane=NULL;
      if(lane_it->second->right_lane!=NULL)
        lane_it->second->right_lane->left_lane=NULL;
      // remove the reference from next lanes
      for(unsigned int i=0;i<lane_it->second->next.size();i++)
        for(other_lane_it=lane_it->second->next[i]->prev.begin();other_lane_it!=lane_it->second->next[i]->prev.end();)
        {
          if((*other_lane_it)==lane)
            other_lane_it=lane_it->second->next[i]->prev.erase(other_lane_it);
          else
            other_lane_it++;
        }
      // remove the reference from prev lanes
      for(unsigned int i=0;i<lane_it->second->prev.size();i++)
        for(other_lane_it=lane_it->second->prev[i]->next.begin();other_lane_it!=lane_it->second->prev[i]->next.end();)
        {
          if((*other_lane_it)==lane)
            other_lane_it=lane_it->second->prev[i]->next.erase(other_lane_it);
          else
            other_lane_it++;
        }
      lane_it=this->lanes.erase(lane_it);
      break;
    }
    else
      lane_it++;
  }
}

void COpendriveRoadSegment::add_nodes(OpenDRIVE::road_type &road_info)
{
  std::map<int,COpendriveLane *>::const_iterator lane_it;
  planView::geometry_iterator geom_it;
  COpendriveRoadNode *new_node;
  TOpendriveWorldPoint node_pose;
  double lane_offset;
  unsigned int index;

  try{
    for(geom_it=road_info.planView().geometry().begin(); geom_it!=road_info.planView().geometry().end(); ++geom_it)
    {
      lane_offset=0.0;
      for(int i=-1;i>=-this->num_right_lanes;i--)
      {
        new_node=new COpendriveRoadNode();
        new_node->set_resolution(this->resolution);
        new_node->set_scale_factor(this->scale_factor);
        this->lanes[i]->set_offset(lane_offset);
        new_node->load(*geom_it,this->lanes[i]);
        if(new_node->get_geometry(0).get_length()>this->min_road_length)
        {
          node_pose=new_node->get_start_pose();
          if(this->parent_road->node_exists_at(node_pose))
          {
            delete new_node;
            new_node=this->parent_road->get_node_at(node_pose);
            new_node->add_lane(*geom_it,this->lanes[i]);
          }
          else
          {
            index=this->parent_road->add_node(new_node);
            new_node->set_index(index);
          }
          new_node->add_parent_segment(this);
          this->add_node(new_node);
          this->lanes[i]->add_node(new_node);
        }
        lane_offset-=this->lanes[i]->width;
      }
    }
    for(geom_it=road_info.planView().geometry().end(); geom_it!=road_info.planView().geometry().begin();)
    {
      geom_it--;
      lane_offset=0.0;
      for(int i=1;i<=this->num_left_lanes;i++)
      {
        new_node=new COpendriveRoadNode();
        new_node->set_resolution(this->resolution);
        new_node->set_scale_factor(this->scale_factor);
        this->lanes[i]->set_offset(lane_offset);
        new_node->load(*geom_it,this->lanes[i]);
        if(new_node->get_geometry(0).get_length()>this->min_road_length)
        {
          node_pose=new_node->get_start_pose();
          if(this->parent_road->node_exists_at(node_pose))
          {
            delete new_node;
            new_node=this->parent_road->get_node_at(node_pose);
            new_node->add_lane(*geom_it,this->lanes[i]);
          }
          else
          {
            index=this->parent_road->add_node(new_node);
            new_node->set_index(index);
          }
          new_node->add_parent_segment(this);
          this->add_node(new_node);
          this->lanes[i]->add_node(new_node);
        }
        lane_offset+=this->lanes[i]->width;
      }
    }
  }catch(CException &e){
    for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
      delete lane_it->second;
    this->lanes.clear();
    throw e;
  }
}

void COpendriveRoadSegment::add_node(COpendriveRoadNode *node)
{
  for(unsigned int i=0;i<this->nodes.size();i++)
    if(this->nodes[i]==node)
      return;
  //add the node
  this->nodes.push_back(node);
}

void COpendriveRoadSegment::remove_node(COpendriveRoadNode *node)
{
  std::vector<COpendriveRoadNode *>::iterator it;

  for(it=this->nodes.begin();it!=this->nodes.end();)
  {
    if((*it)==node)
    {
      it=this->nodes.erase(it);
      break;
    }
    else
      it++;
  }
}

bool COpendriveRoadSegment::has_node(COpendriveRoadNode *node)
{
  for(unsigned int i=0;i<this->nodes.size();i++)
    if(this->nodes[i]==node)
      return true;

  return false;
}

int COpendriveRoadSegment::get_node_side(COpendriveRoadNode *node)
{
  std::map<int,COpendriveLane *>::iterator it;

  if(this->has_node(node))
  {
    for(it=this->lanes.begin();it!=this->lanes.end();it++)
      if(it->second->has_node(node))
        return it->first;

    return 0;
  }
  else
    throw CException(_HERE_,"Road segment does not include the given node");
}

void COpendriveRoadSegment::link_neightbour_lanes(lanes::laneSection_type &lane_section)
{
  center::lane_type center_lane;

  if(lane_section.center().lane().present())
  {
    center_lane=lane_section.center().lane().get();
    if(center_lane.roadMark().size()>1)
      throw CException(_HERE_,"Only one road mark supported for lane");
    else if(center_lane.roadMark().size()==0)
      this->set_center_mark(OD_MARK_NONE);
    else if(center_lane.roadMark().size()==1)
    {
      if(center_lane.roadMark().begin()->type1().present())
      {
        if(center_lane.roadMark().begin()->type1().get()=="none")
          this->set_center_mark(OD_MARK_NONE);
        else if(center_lane.roadMark().begin()->type1().get()=="solid")
          this->set_center_mark(OD_MARK_SOLID);
        else if(center_lane.roadMark().begin()->type1().get()=="broken")
          this->set_center_mark(OD_MARK_BROKEN);
        else if(center_lane.roadMark().begin()->type1().get()=="solid solid")
          this->set_center_mark(OD_MARK_SOLID_SOLID);
        else if(center_lane.roadMark().begin()->type1().get()=="solid broken")
          this->set_center_mark(OD_MARK_SOLID_BROKEN);
        else if(center_lane.roadMark().begin()->type1().get()=="broken solid")
          this->set_center_mark(OD_MARK_BROKEN_SOLID);
        else if(center_lane.roadMark().begin()->type1().get()=="broken broken")
          this->set_center_mark(OD_MARK_BROKEN_BROKEN);
        else
          this->set_center_mark(OD_MARK_NONE);
      }
      else
        this->set_center_mark(OD_MARK_NONE);
    }
  }
  for(int i=-this->num_right_lanes;i<0;i++)
  {
    if(this->lanes.find(i+1)!=this->lanes.end())// if the lane exists 
      this->lanes[i]->link_neightbour_lane(this->lanes[i+1]);
    else
      this->lanes[i]->left_mark=OD_MARK_NONE;
  }
  for(int i=this->num_left_lanes;i>0;i--)
  {
    if(this->lanes.find(i-1)!=this->lanes.end())// if the lane exists 
      this->lanes[i]->link_neightbour_lane(this->lanes[i-1]);
    else
      this->lanes[i]->right_mark=OD_MARK_NONE;
  }

  if(this->lanes.find(1)!=this->lanes.end())
    this->lanes[1]->right_mark=this->center_mark;
  if(this->lanes.find(-1)!=this->lanes.end())
    this->lanes[-1]->left_mark=this->center_mark;
//  this->lanes[-1]->link_neightbour_lane(this->lanes[1]);
}

void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment)
{
  for(unsigned int i=0;i<this->connecting.size();i++)
    if(this->connecting[i]->get_id()==segment.get_id())// the segment is already included
      return;
  for(unsigned int i=0;i<segment.connecting.size();i++)
    if(segment.connecting[i]->get_id()==this->id)
      return;
  this->connecting.push_back(&segment);
  segment.connecting.push_back(this);
  // link lanes
  for(int i=-this->num_right_lanes;i<0;i++)
  {
    for(int j=i-1;j<=i+1;j++)
    {
      if(segment.lanes.find(j)!=segment.lanes.end())
      {
        if(j==i-1)
          this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->right_mark,false,true);
        else if(j==i)
          this->lanes[i]->link_lane(segment.lanes[j],OD_MARK_NONE,false,true);
        else
          this->lanes[i]->link_lane(segment.lanes[j],this->lanes[i]->left_mark,false,true);
      }
    }
  }
  for(int i=1;i<=segment.num_left_lanes;i++)
  {
    for(int j=i-1;j<=i+1;j++)
    {
      if(this->lanes.find(j)!=this->lanes.end())
      {
        if(j==i-1)
          segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->right_mark,false,true);
        else if(j==i)
          segment.lanes[i]->link_lane(this->lanes[j],OD_MARK_NONE,false,true);
        else
          segment.lanes[i]->link_lane(this->lanes[j],segment.lanes[i]->left_mark,false,true);
      }
    }
  }
}

void COpendriveRoadSegment::link_segment(COpendriveRoadSegment &segment,int from,bool from_start,int to,bool to_start)
{
  bool connect=true;

  for(unsigned int i=0;i<this->connecting.size();i++)
  {
    if(this->connecting[i]->get_id()==segment.get_id())// the segment is already included
    {
      for(unsigned int j=0;j<segment.connecting.size();j++)
      {
        if(segment.connecting[j]->get_id()==this->id)
        {
          connect=false;
          break;
        }
      }
    }
  }
  if(connect)
  {
    this->connecting.push_back(&segment);
    segment.connecting.push_back(this);
  }
  // link lanes
  if(segment.lanes.find(to)!=segment.lanes.end())
  {
    if(this->lanes.find(from-1)!=this->lanes.end())
      this->lanes[from-1]->link_lane(segment.lanes[to],this->lanes[from-1]->right_mark,from_start,to_start);
    if(this->lanes.find(from)!=this->lanes.end())
      this->lanes[from]->link_lane(segment.lanes[to],OD_MARK_NONE,from_start,to_start);
    if(this->lanes.find(from+1)!=this->lanes.end())
      this->lanes[from+1]->link_lane(segment.lanes[to],this->lanes[from+1]->left_mark,from_start,to_start);
  }
}

bool COpendriveRoadSegment::connects_to(COpendriveRoadSegment* segment)
{
  for(unsigned int i=0;i<this->connecting.size();i++)
    if(this->connecting[i]==segment)
      return true;

  return false;
}

bool COpendriveRoadSegment::connects_nodes(COpendriveRoadNode *node1,COpendriveRoadNode *node2)
{
  if(this->has_node(node1) && this->has_node(node2))
    return true;
  else
    return false;
}

COpendriveRoadSegment *COpendriveRoadSegment::get_sub_segment(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,int node_side,TOpendriveWorldPoint *start,TOpendriveWorldPoint *end)
{
  std::map<int,COpendriveLane *>::iterator lane_it;
  lane_up_ref_t::iterator lane_ref_it;
  COpendriveLane *new_lane;
  std::vector<COpendriveRoadSegment *>::iterator seg_it;
  segment_up_ref_t new_segment_ref;
  COpendriveRoadSegment *new_segment;
  std::vector<COpendriveRoadNode *>::iterator node_it;
  node_up_ref_t::iterator node_ref_it;
  COpendriveRoadNode *new_node;

  if(start==NULL && end==NULL)
    return this->clone(new_node_ref,new_lane_ref);
  new_segment=new COpendriveRoadSegment(*this);
  new_segment_ref[this]=new_segment;
  /* get the sublanes */
  for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
  {
    if(node_side<0)
      new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,&new_node,start,end);
    else
      new_lane=lane_it->second->get_sub_lane(new_node_ref,new_lane_ref,&new_node,end,start);
    new_lane_ref[lane_it->second]=new_lane;
    if(new_node!=NULL)
      new_segment->add_node(new_node);
  }
  new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);
  // update signals and objects

  return new_segment; 
}

COpendriveRoadSegment *COpendriveRoadSegment::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref)
{
  std::map<int,COpendriveLane *>::iterator lane_it;
  lane_up_ref_t::iterator lane_ref_it;
  COpendriveLane *new_lane;
  segment_up_ref_t new_segment_ref;
  COpendriveRoadSegment *new_segment;
  std::vector<COpendriveRoadNode *>::iterator node_it;
  node_up_ref_t::iterator node_ref_it;

  new_segment=new COpendriveRoadSegment(*this);
  new_segment_ref[this]=new_segment;
  /* get the sublanes */
  for(lane_it=new_segment->lanes.begin();lane_it!=new_segment->lanes.end();lane_it++)
  {
    new_lane=lane_it->second->clone(new_node_ref,new_lane_ref);
    new_lane_ref[lane_it->second]=new_lane;
  }
  new_segment->update_references(new_segment_ref,new_lane_ref,new_node_ref);

  return new_segment;
}

void COpendriveRoadSegment::load(OpenDRIVE::road_type &road_info)
{
  std::map<int,COpendriveLane *>::const_iterator lane_it;
  signals::signal_iterator signal_it;
  objects::object_iterator object_it;
  lanes::laneSection_iterator lane_section;
  COpendriveSignal *new_signal;
  COpendriveObject *new_object;

  this->free();
  this->set_name(road_info.name().get());
  this->set_id(std::stoi(road_info.id().get()));
  // only one lane section is supported
  if(road_info.lanes().laneSection().size()<1)
    throw CException(_HERE_,"No lane sections defined for road "+road_info.id().get());
  else if(road_info.lanes().laneSection().size()>1)
    throw CException(_HERE_,"Road "+road_info.id().get()+" has more than one lane section");
  else
    lane_section=road_info.lanes().laneSection().begin();

  // add lanes
  try{
    this->add_lanes(*lane_section);
    // add road nodes
    this->add_nodes(road_info);
    // link lanes
    this->link_neightbour_lanes(*lane_section);
    // add road signals
    if(road_info.signals().present())
    {
      for(signal_it=road_info.signals().get().signal().begin();signal_it!=road_info.signals().get().signal().end();++signal_it)
      {
        new_signal=new COpendriveSignal();
        new_signal->load(*signal_it,this);
        new_signal->set_scale_factor(this->scale_factor);
        this->signals.push_back(new_signal);
      }
    }
    // add road objects
    if(road_info.objects().present())
    {
      for(object_it=road_info.objects().get().object().begin();object_it!=road_info.objects().get().object().end();++object_it)
      {
        new_object=new COpendriveObject();
        new_object->load(*object_it,this);
        new_object->set_scale_factor(this->scale_factor);
        this->objects.push_back(new_object);
      }
    }
  }catch(CException &e){
    this->free();
    throw e;
  }
}

std::string COpendriveRoadSegment::get_name(void) const
{
  return this->name;
}

unsigned int COpendriveRoadSegment::get_id(void) const
{
  return this->id;
}

unsigned int COpendriveRoadSegment::get_num_right_lanes(void) const
{
  return this->num_right_lanes;
}

unsigned int COpendriveRoadSegment::get_num_left_lanes(void) const
{
  return this->num_left_lanes;
}

const COpendriveLane &COpendriveRoadSegment::get_lane(int index) const
{
  std::map<int,COpendriveLane *>::const_iterator lane_it;

  for(lane_it=this->lanes.begin();lane_it!=this->lanes.end();++lane_it)
  {
    if(lane_it->second->get_id()==index)
      return *lane_it->second;
  }
  throw CException(_HERE_,"invalid lane index");
}

unsigned int COpendriveRoadSegment::get_num_nodes(void) const
{
  return this->nodes.size();
}

const COpendriveRoadNode &COpendriveRoadSegment::get_node(unsigned int index) const
{
  if(index>=0 && index<this->nodes.size())
    return *this->nodes[index];
  else
    throw CException(_HERE_,"Invalid signal index");
}

const COpendriveRoad &COpendriveRoadSegment::get_parent_road(void) const
{
  return *this->parent_road;
}

unsigned int COpendriveRoadSegment::get_num_signals(void) const
{
  return this->signals.size();  
}

const COpendriveSignal &COpendriveRoadSegment::get_signal(unsigned int index) const
{
  if(index>=0 && index<this->signals.size())
    return *this->signals[index];
  else
    throw CException(_HERE_,"Invalid signal index");
}

unsigned int COpendriveRoadSegment::get_num_objects(void) const
{
  return this->objects.size();
}

const COpendriveObject &COpendriveRoadSegment::get_object(unsigned int index) const
{
  if(index>=0 && index<this->objects.size())
    return *this->objects[index];
  else
    throw CException(_HERE_,"Invalid object index");
}

unsigned int COpendriveRoadSegment::get_num_connecting(void) const
{
  return this->connecting.size();
}

const COpendriveRoadSegment &COpendriveRoadSegment::get_connecting(unsigned int index) const
{
  if(index>=0 && index<this->connecting.size())
    return *this->connecting[index];
  else
    throw CException(_HERE_,"Invalid connecting segment index");
}

TOpendriveLocalPoint COpendriveRoadSegment::transform_to_local_point(const TOpendriveTrackPoint &track)
{
  TOpendriveTrackPoint point;
  TOpendriveLocalPoint local;

  if(track.s<0.0)
    throw CException(_HERE_,"Invalid track point");
  point=track;
  if(this->num_right_lanes>0)
    local=this->lanes[-1]->transform_to_local_point(point);
  else
  {
    point.s=this->lanes[1]->get_length()-track.s;
    point.heading=normalize_angle(track.heading+3.14159);
    local=this->lanes[1]->transform_to_local_point(point);
  }

  return local;
}

TOpendriveWorldPoint COpendriveRoadSegment::transform_to_world_point(const TOpendriveTrackPoint &track)
{
  TOpendriveTrackPoint point;
  TOpendriveWorldPoint world;

  if(track.s<0.0)
    throw CException(_HERE_,"Invalid track point");
  point=track;
  if(this->num_right_lanes>0)
    world=this->lanes[-1]->transform_to_world_point(point);
  else
  {
    point.s=this->lanes[1]->get_length()-track.s;
    point.heading=normalize_angle(track.heading+3.14159);
    world=this->lanes[1]->transform_to_world_point(point);
  }

  return world;
}

std::ostream& operator<<(std::ostream& out, COpendriveRoadSegment &segment)
{
  out << "Road " << segment.get_name() << " (" << segment.get_id() << ")" << std::endl;
  out << "  Connecting road segments: " << segment.connecting.size() << std::endl;
  for(unsigned int i=0;i<segment.connecting.size();i++)
    out << "    " << segment.connecting[i]->get_name() << std::endl;
  out << "  Number of nodes: " << segment.get_num_nodes() << std::endl;
  for(unsigned int i=0;i<segment.get_num_nodes();i++)
    out << "    " << segment.get_node(i).get_index() << std::endl;
  out << "  Number of right lanes: " << segment.num_right_lanes << std::endl;
  for(int i=-1;i>=-segment.num_right_lanes;i--)
    out << *segment.lanes[i];
  out << "  Number of left lanes: " << segment.num_left_lanes << std::endl;
  for(int i=1;i<=segment.num_left_lanes;i++)
    out << *segment.lanes[i];
  out << "  Number of signals: " << segment.signals.size() << std::endl;
  for(unsigned int i=0;i<segment.signals.size();i++)
    std::cout << *segment.signals[i];
  out << "  Number of objects: " << segment.objects.size() << std::endl;
  for(unsigned int i=0;i<segment.objects.size();i++)
    std::cout << *segment.objects[i];

  return out;
}

COpendriveRoadSegment::~COpendriveRoadSegment()
{
  this->free();
  this->name="";
  this->id=-1;
  this->resolution=DEFAULT_RESOLUTION;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
  this->min_road_length=DEFAULT_MIN_ROAD_LENGTH;
}