opendrive_lane.cpp

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

COpendriveLane::COpendriveLane()
{
  this->nodes.clear();
  this->next.clear();
  this->prev.clear();
  this->left_lane=NULL;
  this->left_mark=OD_MARK_NONE;
  this->right_lane=NULL;
  this->right_mark=OD_MARK_NONE;
  this->segment=NULL;
  this->resolution=DEFAULT_RESOLUTION;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
  this->width=0.0;
  this->speed=0.0;
  this->offset=0.0;
  this->id=0;
  this->index=-1;
}

COpendriveLane::COpendriveLane(const COpendriveLane &object)
{
  this->nodes=object.nodes;
  this->next=object.next;
  this->prev=object.prev;
  this->left_lane=object.left_lane;
  this->left_mark=object.left_mark;
  this->right_lane=object.right_lane;
  this->right_mark=object.right_mark;
  this->segment=object.segment;
  this->resolution=object.resolution;
  this->scale_factor=object.scale_factor;
  this->width=object.width;
  this->speed=object.speed;
  this->offset=object.offset;
  this->id=object.id;
  this->index=object.index;
}

void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
{
  unsigned int min_num_nodes;

  if(lane!=NULL)
  {
    if(this->get_num_nodes()<lane->get_num_nodes())
      min_num_nodes=this->get_num_nodes();
    else
      min_num_nodes=lane->get_num_nodes();
    for(unsigned int i=0;i<min_num_nodes-1;i++)
    {
      this->nodes[i]->add_link(lane->nodes[i+1],lane->right_mark,this->segment,NULL);
      lane->nodes[i]->add_link(this->nodes[i+1],lane->right_mark,this->segment,NULL);
      this->left_mark=lane->right_mark;
    } 
    if(min_num_nodes>0)
    {
      this->left_lane=lane;
      lane->right_lane=this;
    }
  }
}

void COpendriveLane::link_lane(COpendriveLane *lane,opendrive_mark_t mark,bool from_start, bool to_start,bool belongs_to_lane)
{
  COpendriveRoadNode *start,*end;
  std::stringstream error;

  if(lane!=NULL)
  {
    if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
    {
      if(from_start)
      {
        if(this->id<0)
          start=this->nodes[0];
        else
          start=this->nodes[this->nodes.size()-1];
        if(to_start)
        {
          if(lane->id<0)
            end=lane->nodes[0];
          else
            end=lane->nodes[lane->nodes.size()-1];
        }
        else
        {
          if(lane->id<0)
            end=lane->nodes[lane->nodes.size()-1];
          else
            end=lane->nodes[0];
        }
      }
      else
      {
        if(this->id<0)
          start=this->nodes[this->nodes.size()-1];
        else
          start=this->nodes[0];
        if(to_start)
        {
          if(lane->id<0)
            end=lane->nodes[0];
          else
            end=lane->nodes[lane->nodes.size()-1];
        }
        else
        {
          if(lane->id<0)
            end=lane->nodes[lane->nodes.size()-1];
          else
            end=lane->nodes[0];
        }
      }
      if(belongs_to_lane)
        start->add_link(end,mark,this->segment,this);
      else
        start->add_link(end,mark,this->segment,NULL);
      // link lane
      this->add_next_lane(lane);
      lane->add_prev_lane(this);
    }
    else 
    {
      error << "Lane " << this->id << " of segment " << this->segment->get_name() << " or lane " << lane->get_id() << " of segment " << lane->get_segment().get_name() << " has no nodes";
      throw CException(_HERE_,error.str());
    }
  }
}

void COpendriveLane::add_next_lane(COpendriveLane *lane)
{
  for(unsigned int i=0;i<this->next.size();i++)
    if(this->next[i]==lane)// lane is already linked
      return;

  // add the lane
  this->next.push_back(lane);
}

void COpendriveLane::add_prev_lane(COpendriveLane *lane)
{
  for(unsigned int i=0;i<this->prev.size();i++)
    if(this->prev[i]==lane)// lane is already linked
      return;

  // add the lane
  this->prev.push_back(lane);
}

void COpendriveLane::remove_lane(COpendriveLane *lane)
{
  std::vector<COpendriveLane *>::iterator lane_it;

  // remove the reference from neightbour lanes
  if(this->right_lane==lane)
    this->right_lane=NULL;
  if(this->left_lane==lane)
    this->left_lane=NULL;
  // remove the reference from next lanes
  for(lane_it=this->next.begin();lane_it!=this->next.end();)
  {
    if((*lane_it)==lane)
      lane_it=this->next.erase(lane_it);
     else
      lane_it++;
  }
  // remove the reference from next lanes
  for(lane_it=this->prev.begin();lane_it!=this->prev.end();)
  {
    if((*lane_it)==lane)
      lane_it=this->prev.erase(lane_it);
     else
      lane_it++;
  }
}

void COpendriveLane::add_node(COpendriveRoadNode *node)
{
  if(this->has_node(node))
    return;
  // link the new node with the previous one in the current lane, if any
  if(this->nodes.size()>0)
    this->nodes[this->nodes.size()-1]->add_link(node,OD_MARK_NONE,this->segment,this);
  // add the new node
  this->nodes.push_back(node);
}

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

  return false;
}

bool COpendriveLane::has_node_with_index(unsigned int index)
{
  for(unsigned int i=0;i<this->nodes.size();i++)
    if(this->nodes[i]->get_index()==index)
      return true;

  return false;
}

void COpendriveLane::set_parent_segment(COpendriveRoadSegment *segment)
{
  this->segment=segment;
}

void COpendriveLane::set_left_lane(COpendriveLane *lane,opendrive_mark_t mark)
{
  this->left_lane=lane;
  this->left_mark=mark;
}

void COpendriveLane::set_right_lane(COpendriveLane *lane,opendrive_mark_t mark)
{
  this->right_lane=lane;
  this->right_mark=mark;
}

void COpendriveLane::set_resolution(double res)
{
  this->resolution=res;

  for(unsigned int i=0;i<this->nodes.size();i++)
    this->nodes[i]->set_resolution(res);
}

void COpendriveLane::set_scale_factor(double scale)
{ 
  this->scale_factor=scale;

  for(unsigned int i=0;i<this->nodes.size();i++)
    this->nodes[i]->set_scale_factor(scale);
}

void COpendriveLane::set_width(double width)
{
  this->width=width;
}

void COpendriveLane::set_speed(double speed)
{
  this->speed=speed;
}

void COpendriveLane::set_offset(double offset)
{
  this->offset=offset;
}

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

void COpendriveLane::set_index(unsigned int index)
{
  this->index=index;
}

bool COpendriveLane::updated(lane_up_ref_t &refs)
{
  lane_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 COpendriveLane::update_references(segment_up_ref_t &segment_refs,lane_up_ref_t &lane_refs,node_up_ref_t &node_refs)
{
  std::vector<COpendriveLane *>::iterator lane_it;
  std::vector<COpendriveRoadNode *>::iterator node_it;
 
  if(this->updated(lane_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];
        (*node_it)->update_references(segment_refs,lane_refs,node_refs);
      }
      else if((*node_it)->updated(node_refs))
        (*node_it)->update_references(segment_refs,lane_refs,node_refs);
    }
    for(lane_it=this->next.begin();lane_it!=this->next.end();lane_it++)
      if(lane_refs.find(*lane_it)!=lane_refs.end())
        (*lane_it)=lane_refs[*lane_it];
    for(lane_it=this->prev.begin();lane_it!=this->prev.end();lane_it++)
      if(lane_refs.find(*lane_it)!=lane_refs.end())
        (*lane_it)=lane_refs[*lane_it];
    if(lane_refs.find(this->left_lane)!=lane_refs.end())
      this->left_lane=lane_refs[this->left_lane];
    if(lane_refs.find(this->right_lane)!=lane_refs.end())
      this->right_lane=lane_refs[this->right_lane];
    if(segment_refs.find(this->segment)!=segment_refs.end())
      this->segment=segment_refs[this->segment];
  }
}

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

  if(this->updated(lane_refs))
  {
    for(node_it=this->nodes.begin();node_it!=this->nodes.end();)
    {
      if((*node_it)->updated(node_refs))
      {
        (*node_it)->clean_references(segment_refs,lane_refs,node_refs);
        node_it++;
      }
      else
        node_it=this->nodes.erase(node_it);
    }
    for(lane_it=this->next.begin();lane_it!=this->next.end();)
    {
      if(!(*lane_it)->updated(lane_refs))
        lane_it=this->next.erase(lane_it);
      else
        lane_it++;
    }
    for(lane_it=this->prev.begin();lane_it!=this->prev.end();)
    {
      if(!(*lane_it)->updated(lane_refs))
        lane_it=this->prev.erase(lane_it);
      else
        lane_it++;
    }
    if(lane_refs.find(this->left_lane)!=lane_refs.end())
      this->left_lane=lane_refs[this->left_lane];
    else if(!this->left_lane->updated(lane_refs))
      this->left_lane=NULL;
    if(lane_refs.find(this->right_lane)!=lane_refs.end())
      this->right_lane=lane_refs[this->right_lane];
    else if(!this->right_lane->updated(lane_refs))
      this->right_lane=NULL;
  }
}

void COpendriveLane::update_start_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start)
{
  std::vector<COpendriveRoadNode *>::iterator it;
  segment_up_ref_t new_segment_ref;
  unsigned int start_node_index,i;
  TOpendriveWorldPose start_pose;
  COpendriveRoadNode *new_node;
  std::stringstream error;
  double distance;
  
  if(start==NULL)
    return;
  start_node_index=this->get_closest_node_index(*start,distance,3.14159);
  if(start_node_index==(unsigned int)-1)
  {
    error << "No node close to (x=" << start->x << ",y=" << start->y << ",heading=" << start->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
    throw CException(_HERE_,error.str());
  }
  // get the actual start position on the lane
  this->get_closest_pose(*start,start_pose,3.14159);
  // eliminate all the node before the start one
  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
  {
    if(i<start_node_index)
    {
      if((*it)->updated(new_node_ref))
      {
        new_node_ref.erase((*it)->get_original_node(new_node_ref));
        delete *it;
      }
      it=this->nodes.erase(it);
    }
    else
    {
      if(!(*it)->updated(new_node_ref))
      {
        new_node=(*it)->clone(new_link_ref);
        new_node_ref[*it]=new_node;
      }
      it++;
    }
  }
  this->prev.clear();// it is no longer connected to any previous lane
  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
  this->nodes[0]->update_start_pose(this,start_pose,distance);
}

void COpendriveLane::update_end_node(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *end)
{
  std::vector<COpendriveRoadNode *>::iterator it;
  segment_up_ref_t new_segment_ref;
  unsigned int end_node_index,i;
  TOpendriveWorldPose end_pose;
  COpendriveRoadNode *new_node;
  std::stringstream error;
  double distance;
    
  if(end==NULL)
    return;
  end_node_index=this->get_closest_node_index(*end,distance,3.14159);
  if(end_node_index==(unsigned int)-1)
  {
    error << "No node close to (x=" << end->x << ",y=" << end->y << ",heading=" << end->heading << ") in lane " << this->id << " of segment " << this->segment->get_name();
    throw CException(_HERE_,error.str());
  }
  this->get_closest_pose(*end,end_pose,3.14159);
  // eliminate all the node before the start one
  for(it=this->nodes.begin(),i=0;it!=this->nodes.end();i++)
  {
    if(i>end_node_index)
    {
      if((*it)->updated(new_node_ref))
      {
        new_node_ref.erase((*it)->get_original_node(new_node_ref));
        delete *it;
      }
      it=this->nodes.erase(it);
    }
    else
    {
      if(!(*it)->updated(new_node_ref))
      {
        new_node=(*it)->clone(new_link_ref);
        new_node_ref[*it]=new_node;
      }
      it++;
    }
  }
  this->next.clear();// it is no longer connected to any next lane
  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);
  this->nodes[this->nodes.size()-1]->update_end_pose(this,end_pose,distance);
}

COpendriveLane *COpendriveLane::get_sub_lane(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref,TOpendriveWorldPose *start,TOpendriveWorldPose *end)
{
  COpendriveLane *new_lane;

  if(start==NULL && end==NULL)
    return this->clone(new_node_ref,new_lane_ref,new_link_ref);
  new_lane=new COpendriveLane(*this);
  new_lane_ref[this]=new_lane;
  if(this->id<0)
  {
    new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,start);
    new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,end);
  }
  else
  {
    new_lane->update_start_node(new_node_ref,new_lane_ref,new_link_ref,end);
    new_lane->update_end_node(new_node_ref,new_lane_ref,new_link_ref,start);
  }

  return new_lane;
}

COpendriveLane *COpendriveLane::clone(node_up_ref_t &new_node_ref,lane_up_ref_t &new_lane_ref,link_up_ref_t &new_link_ref)
{
  COpendriveLane *new_lane;
  std::vector<COpendriveRoadNode *>::iterator it;
  COpendriveRoadNode *new_node;
  segment_up_ref_t new_segment_ref;

  new_lane=new COpendriveLane(*this);
  new_lane_ref[this]=new_lane;
  for(it=new_lane->nodes.begin();it!=new_lane->nodes.end();it++)
  {
    new_node=(*it)->clone(new_link_ref);
    new_node_ref[*it]=new_node;
  }
  this->update_references(new_segment_ref,new_lane_ref,new_node_ref);

  return new_lane;
}

void COpendriveLane::load(const ::lane &lane_info,COpendriveRoadSegment *segment)
{
  std::stringstream error;
  opendrive_mark_t mark;

  this->nodes.clear();
  this->prev.clear();
  this->next.clear();
  this->left_lane=NULL;
  this->left_mark=OD_MARK_NONE;
  this->right_lane=NULL;
  this->right_mark=OD_MARK_NONE;
  this->set_id(lane_info.id().get());
  // import lane width
  if(lane_info.width().size()<1)
  {
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has no width record";
    throw CException(_HERE_,error.str());
  }
  else if(lane_info.width().size()>1)
  {
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one width record";
    throw CException(_HERE_,error.str());
  }
  this->set_width(lane_info.width().begin()->a().get());
  // import lane speed
  if(lane_info.speed().size()<1)
  {
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has no speed record";
    throw CException(_HERE_,error.str());
  }
  else if(lane_info.speed().size()>1)
  {
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one speed record";
    throw CException(_HERE_,error.str());
  }
  this->set_speed(lane_info.speed().begin()->max().get());
  //lane mark
  if(lane_info.roadMark().size()==0)
    mark=OD_MARK_NONE;
  else if(lane_info.roadMark().size()>1)
  {
    error << "Lane " << this->id << " of segment " << segment->get_name() << " has more than one road mark record";
    throw CException(_HERE_,error.str());
  }
  else if(lane_info.roadMark().size()==1)
  {
    if(lane_info.roadMark().begin()->type1().present())
    {
      if(lane_info.roadMark().begin()->type1().get()=="none")
        mark=OD_MARK_NONE;
      else if(lane_info.roadMark().begin()->type1().get()=="solid")
        mark=OD_MARK_SOLID;
      else if(lane_info.roadMark().begin()->type1().get()=="broken")
        mark=OD_MARK_BROKEN;
      else if(lane_info.roadMark().begin()->type1().get()=="solid solid")
        mark=OD_MARK_SOLID_SOLID;
      else if(lane_info.roadMark().begin()->type1().get()=="solid broken")
        mark=OD_MARK_SOLID_BROKEN;
      else if(lane_info.roadMark().begin()->type1().get()=="broken solid")
        mark=OD_MARK_BROKEN_SOLID;
      else if(lane_info.roadMark().begin()->type1().get()=="broken broken")
        mark=OD_MARK_BROKEN_BROKEN;
      else
        mark=OD_MARK_NONE;
    }
    else
      mark=OD_MARK_NONE;
  }
  this->right_mark=mark;

  this->set_parent_segment(segment);
}

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

const COpendriveRoadNode &COpendriveLane::get_node(unsigned int index) const
{
  std::stringstream error;

  if(index>=0 && index<this->nodes.size())
    return *this->nodes[index];
  else
  {
    error << "Invalid node index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
    throw CException(_HERE_,error.str());
  }
}

unsigned int COpendriveLane::get_num_next_lanes(void) const
{
  return this->next.size();
}

const COpendriveLane &COpendriveLane::get_next_lane(unsigned int index) const
{
  std::stringstream error;

  if(index>=0 && index<this->next.size())
    return *this->next[index];
  else
  {
    error << "Invalid next lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
    throw CException(_HERE_,error.str());
  }
}

unsigned int COpendriveLane::get_num_prev_lanes(void) const
{
  return this->prev.size();
}

const COpendriveLane &COpendriveLane::get_prev_lane(unsigned int index) const
{
  std::stringstream error;

  if(index>=0 && index<this->prev.size())
    return *this->prev[index];
  else
  {
    error << "Invalid previous lane index (" << index << ") for segment " << this->segment->get_name() << " (lane " << this->id << ")"; 
    throw CException(_HERE_,error.str());
  }
}

const COpendriveRoadSegment &COpendriveLane::get_segment(void) const
{
  return *this->segment;
}

double COpendriveLane::get_width(void) const
{
  return this->width/this->scale_factor;
}

double COpendriveLane::get_speed(void) const
{
  return this->speed;
}

double COpendriveLane::get_offset(void) const
{
  return this->offset/this->scale_factor;
}

double COpendriveLane::get_center_offset(void) const
{
  if(this->id<0)
    return (this->offset-this->width/2.0)/this->scale_factor;
  else
    return (this->offset+this->width/2.0)/this->scale_factor;
}

unsigned int COpendriveLane::get_index(void) const
{
  return this->index;
}

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

TOpendriveWorldPose COpendriveLane::get_start_pose(void) const
{
  TOpendriveTrackPose track_pose;
  TOpendriveWorldPose world_pose;
  std::stringstream error;

  track_pose.t=0.0;
  if(this->id<0)// right lane
  {
    track_pose.s=0.0;
    track_pose.heading=0.0;
  }
  else
  {
    track_pose.s=this->segment->get_length();
    track_pose.heading=3.14159;
  }
  world_pose=this->transform_to_world_pose(track_pose);

  return world_pose;
}

TOpendriveWorldPose COpendriveLane::get_end_pose(void) const
{
  TOpendriveTrackPose track_pose;
  TOpendriveWorldPose world_pose;
  std::stringstream error;
  
  track_pose.t=0.0;
  if(this->id<0)// right_lane
  {
    track_pose.s=this->segment->get_length();
    track_pose.heading=0.0;
  }
  else
  {
    track_pose.s=0.0;
    track_pose.heading=3.14159;
  }
  world_pose=this->transform_to_world_pose(track_pose);

  return world_pose;
}

double COpendriveLane::get_length(void) const
{
  std::stringstream error;
  double length=0.0;

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
    {
      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
        length+=this->nodes[i]->links[j]->get_length();
    }
  }

  return length;
}

TOpendriveWorldPose COpendriveLane::get_pose_at(double length)
{
  TOpendriveWorldPose world_pose={0};

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
    {
      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
      {
        if((length-this->nodes[i]->links[j]->get_length())<0.0)
        {
          world_pose=this->nodes[i]->links[j]->get_pose_at(length);
          return world_pose;
        }
        else
          length-=this->nodes[i]->links[j]->get_length();
      }
    }
  }

  return world_pose;
}

double COpendriveLane::get_curvature_at(double length)
{
  double curvature;

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    for(unsigned int j=0;j<this->nodes[i]->get_num_links();j++)
    {
      if(&this->nodes[i]->get_link(j).get_parent_lane()==this)
      {
        if((length-this->nodes[i]->links[j]->get_length())<0.0)
        {
          curvature=this->nodes[i]->links[j]->get_curvature_at(length);
          return curvature;
        }
        else
          length-=this->nodes[i]->links[j]->get_length();
      }
    }
  }

  return 0.0;
}

TOpendriveLocalPose COpendriveLane::transform_to_local_pose(TOpendriveTrackPose &track) const
{
  TOpendriveTrackPose pose;

  pose=track;
  pose.t+=this->get_center_offset();
  return this->segment->transform_to_local_pose(pose);
}

TOpendriveWorldPose COpendriveLane::transform_to_world_pose(TOpendriveTrackPose &track) const
{ 
  TOpendriveTrackPose pose;

  pose=track;
  pose.t+=this->get_center_offset();
  return this->segment->transform_to_world_pose(pose);
}

unsigned int COpendriveLane::get_closest_node_index(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  double dist,min_dist=std::numeric_limits<double>::max(),length;
  TOpendriveWorldPose found_pose;
  unsigned int closest_index=-1;

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
    if(length<std::numeric_limits<double>::max())
    {
      dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
      if(dist<min_dist)
      {
        min_dist=dist;
        closest_index=i;
        distance=length;
      }
    }
  }

  return closest_index;
}

const COpendriveRoadNode &COpendriveLane::get_closest_node(TOpendriveWorldPose &pose,double &distance,double angle_tol) const
{
  unsigned int closest_index;

  closest_index=this->get_closest_node_index(pose,distance,angle_tol);
  if(closest_index==(unsigned int)-1)
    throw CException(_HERE_,"Impossible to find the closest node");
  return *this->nodes[closest_index];
}

double COpendriveLane::get_closest_pose(TOpendriveWorldPose &pose,TOpendriveWorldPose &closest_pose,double angle_tol) const
{
  double dist,min_dist=std::numeric_limits<double>::max(),distance,length;
  TOpendriveWorldPose found_pose;
  unsigned int closest_index=-1;
  COpendriveLink *link;

  closest_pose.x=std::numeric_limits<double>::max();
  closest_pose.y=std::numeric_limits<double>::max();
  closest_pose.heading=std::numeric_limits<double>::max();
  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    length=this->nodes[i]->get_closest_pose(pose,found_pose,true,angle_tol);
    if(length<std::numeric_limits<double>::max())
    {
      dist=sqrt(pow(found_pose.x-pose.x,2.0)+pow(found_pose.y-pose.y,2.0));
      if(dist<min_dist)
      {
        min_dist=dist;
        closest_index=i;
        closest_pose=found_pose;
        distance=length;
      }
    }
  }

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    if(i<closest_index)
    {
      link=this->nodes[i]->get_link_with(this->nodes[i+1]);
      if(link!=NULL)
        distance+=link->get_length();
    }
    else
      break;
  }

  return distance;
}

std::ostream& operator<<(std::ostream& out, COpendriveLane &lane)
{
  out << "  Lane: " << lane.id << " (offset: " << lane.offset << ")" << std::endl;
  out << "    width: " << lane.get_width() << std::endl;
  out << "    speed: " << lane.get_speed() << std::endl;
  if(lane.segment!=NULL)
    out << "    Parent road segment: " << lane.segment->get_name() << std::endl;
  else 
    out << "    No parent segment" << std::endl;
  if(lane.left_lane==NULL)
    out << "    No left lane (";
  else
    out << "    Left lane " << lane.left_lane->get_id() << " (";
  switch(lane.left_mark)
  {
    case OD_MARK_NONE:
      out << "no mark)" << std::endl;
      break;
    case OD_MARK_SOLID:
      out << "solid)" << std::endl;
      break;
    case OD_MARK_BROKEN:
      out << "broken)" << std::endl;
      break;
    case OD_MARK_SOLID_SOLID:
      out << "solid solid)" << std::endl;
      break;
    case OD_MARK_SOLID_BROKEN:
      out << "solid broken)" << std::endl;
      break;
    case OD_MARK_BROKEN_SOLID:
      out << "broken solid)" << std::endl;
      break;
    case OD_MARK_BROKEN_BROKEN:
      out << "broken broken)" << std::endl;
      break;
  }
  if(lane.right_lane==NULL)
    out << "    No right lane (";
  else
    out << "    Right lane " << lane.right_lane->get_id() << " (";
  switch(lane.right_mark)
  {
    case OD_MARK_NONE:
      out << "no mark)" << std::endl;
      break;
    case OD_MARK_SOLID:
      out << "solid)" << std::endl;
      break;
    case OD_MARK_BROKEN:
      out << "broken)" << std::endl;
      break;
    case OD_MARK_SOLID_SOLID:
      out << "solid solid)" << std::endl;
      break;
    case OD_MARK_SOLID_BROKEN:
      out << "solid broken)" << std::endl;
      break;
    case OD_MARK_BROKEN_SOLID:
      out << "broken solid)" << std::endl;
      break;
    case OD_MARK_BROKEN_BROKEN:
      out << "broken broken)" << std::endl;
      break;
  }
  out << "    Number of nodes: " << lane.nodes.size() << std::endl;
  for(unsigned int i=0;i<lane.nodes.size();i++)
    out << *lane.nodes[i];

  return out;
}

COpendriveLane::~COpendriveLane()
{
  this->nodes.clear();
  this->prev.clear();
  this->next.clear();
  this->left_lane=NULL;
  this->left_mark=OD_MARK_NONE;
  this->right_lane=NULL;
  this->right_mark=OD_MARK_NONE;
  this->segment=NULL;
  this->resolution=DEFAULT_RESOLUTION;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
  this->width=0.0;
  this->speed=0.0;
  this->offset=0.0;
  this->id=0;
  this->index=-1;
}