opendrive_lane.cpp

#include "opendrive_lane.h"
#include "exceptions.h"

COpendriveLane::COpendriveLane()
{
  this->nodes.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;
}

COpendriveLane::COpendriveLane(const COpendriveLane &object,std::map<COpendriveRoadNode *,COpendriveRoadNode *> &node_refs,COpendriveRoadSegment *segment_ref)
{
  this->nodes.resize(object.nodes.size());
  for(unsigned int i=0;i<object.nodes.size();i++)
    this->nodes[i]=node_refs[object.nodes[i]];
  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=segment_ref;
  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;
}

void COpendriveLane::link_neightbour_lane(COpendriveLane *lane)
{
  if(lane!=NULL)
  {
    if(this->get_num_nodes()!=lane->get_num_nodes())
      throw CException(_HERE_,"Impossible to liink lanes because they have different number of nodes");
    if(this->get_num_nodes()>0 && lane->get_num_nodes()>0)
    {
      if(this->id*lane->get_id()<0) // oposite directions
      {
        for(unsigned int i=0;i<this->get_num_nodes();i++)
        {
          this->nodes[i]->add_link(lane->nodes[lane->get_num_nodes()-i-1],this->left_mark);
          lane->nodes[lane->get_num_nodes()-i-1]->add_link(this->nodes[i],this->left_mark);
          lane->right_mark=this->left_mark;
        }
      }
      else
      {
        for(unsigned int i=0;i<this->get_num_nodes()-1;i++)
        {
          this->nodes[i]->add_link(lane->nodes[i+1],this->left_mark);
          lane->nodes[i]->add_link(this->nodes[i+1],this->left_mark);
          if(this->id>0)// left lanes
            this->right_mark=lane->left_mark;
          else// right lanes
            this->left_mark=lane->right_mark;
        }
      } 
      this->left_lane=lane;
      lane->right_lane=this;
    }
    else
      throw CException(_HERE_,"One of the lanes to link has no nodes");
  }
}

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

  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];
        }
      }
      start->add_link(end,mark);
    }
    else 
      throw CException(_HERE_,"One of the lanes to link has no nodes");
  }
}

void COpendriveLane::add_node(COpendriveRoadNode *node)
{
  for(unsigned int i=0;i<this->nodes.size();i++)
    if(this->nodes[i]==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);
  // add the new node
  this->nodes.push_back(node);
}

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_offset(double offset)
{
  this->offset=offset;
}

void COpendriveLane::update_references(std::map<COpendriveLane *,COpendriveLane *> &lane_refs)
{
  this->left_lane=lane_refs[this->left_lane];
  this->right_lane=lane_refs[this->right_lane];

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

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

  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    delete this->nodes[i];
    this->nodes[i]=NULL;
  }
  this->nodes.clear();
  this->left_lane=NULL;
  this->left_mark=OD_MARK_NONE;
  this->right_lane=NULL;
  this->right_mark=OD_MARK_NONE;
  this->id=lane_info.id().get();
  // import lane width
  if(lane_info.width().size()!=1)
  {
    error << "Only one width record supported for lane " << this->id;
    throw CException(_HERE_,error.str());
  }
  this->width=lane_info.width().begin()->a().get();
  // import lane speed
  if(lane_info.speed().size()!=1)
  {
    error << "Only one speed record supported for lane " << this->id;
    throw CException(_HERE_,error.str());
  }
  this->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 << "Only one road mark supported for lane " << this->id;
    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;
  }
  if(this->id<0)//right lanes
    this->right_mark=mark;
  else
    this->left_mark=mark;

  this->segment=segment;
}

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

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

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;
}

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

TOpendriveWorldPoint COpendriveLane::get_start_point(void) const
{
  TOpendriveTrackPoint track_point;
  TOpendriveWorldPoint world_point;

  track_point.heading=0.0;
  if(this->id<0)// right lane
  {
    track_point.t=this->get_offset()-this->get_width()/2.0;
    track_point.s=0.0;
    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
      if(&this->nodes[0]->get_lane(i)==this)
        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
  }
  else
  {
    track_point.t=this->get_offset()+this->get_width()/2.0;
    track_point.s=0.0;
    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
        this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
  }
  if(this->get_id()>0)
    world_point.heading=normalize_angle(world_point.heading+3.14159);

  return world_point;
}

TOpendriveWorldPoint COpendriveLane::get_end_point(void) const
{
  TOpendriveTrackPoint track_point;
  TOpendriveWorldPoint world_point;
  
  track_point.heading=0.0;
  if(this->id>0)// left lane
  { 
    track_point.t=this->get_offset()+this->get_width()/2.0;
    for(unsigned int i=0;i<this->nodes[0]->get_num_lanes();i++)
      if(&this->nodes[0]->get_lane(i)==this)
      {
        track_point.s=this->nodes[0]->get_geometry(i).get_length();;
        this->nodes[0]->get_geometry(i).get_world_pose(track_point,world_point);
      }
  }
  else
  { 
    track_point.t=this->get_offset()-this->get_width()/2.0;
    for(unsigned int i=0;i<this->nodes[this->nodes.size()-1]->get_num_lanes();i++)
      if(&this->nodes[this->nodes.size()-1]->get_lane(i)==this)
      {
        track_point.s=this->nodes[this->nodes.size()-1]->get_geometry(i).get_length();
        this->nodes[this->nodes.size()-1]->get_geometry(i).get_world_pose(track_point,world_point);
      }
  }
  if(this->get_id()>0)
    world_point.heading=normalize_angle(world_point.heading+3.14159);

  return world_point;
}

double COpendriveLane::get_length(void) const
{
  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_lanes();j++)
    {
      if(this->nodes[i]->get_lane(j).get_id()==this->id)
        length+=this->nodes[i]->get_geometry(j).get_length();
    }
  }

  return length;
}

TOpendriveLocalPoint COpendriveLane::transform_to_local_point(TOpendriveTrackPoint &track)
{
  TOpendriveLocalPoint local;

  if(track.s<0.0)
    throw CException(_HERE_,"Invalid track point");
  for(unsigned int i=0;i<this->nodes.size();i++)
  { 
    for(unsigned int j=0;j<this->nodes[i]->get_num_lanes();j++)
    { 
      if(this->nodes[i]->get_lane(j).get_id()==this->id)
      { 
        if(track.s<=this->nodes[i]->get_geometry(j).get_length())
        { 
          if(!this->nodes[i]->get_geometry(j).get_local_pose(track,local))
            throw CException(_HERE_,"Impossible to transform to local coordinates");
          return local;
        }
        else
          track.s-=this->nodes[i]->get_geometry(j).get_length();
      }
    }
  }

  throw CException(_HERE_,"Track point does not belong to lane");
}

TOpendriveWorldPoint COpendriveLane::transform_to_world_point(TOpendriveTrackPoint &track)
{ 
  TOpendriveWorldPoint world;

  if(track.s<0.0)
    throw CException(_HERE_,"Invalid track point");
  for(unsigned int i=0;i<this->nodes.size();i++)
  {
    for(unsigned int j=0;j<this->nodes[i]->get_num_lanes();j++)
    {
      if(this->nodes[i]->get_lane(j).get_id()==this->id)
      {
        if(track.s<=this->nodes[i]->get_geometry(j).get_length())
        {
          if(!this->nodes[i]->get_geometry(j).get_world_pose(track,world))
            throw CException(_HERE_,"Impossible to transform to world coordinates");
          return world;
        }
        else
          track.s-=this->nodes[i]->get_geometry(j).get_length();
      }
    }
  }

  throw CException(_HERE_,"Track point does not belong to lane");
}

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->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;
}