opendrive_geometry.cpp

#include "opendrive_geometry.h"
#include <cmath>

COpendriveGeometry::COpendriveGeometry()
{
  this->min_s = 0.0;
  this->max_s = 0.0;
  this->pose.x = 0.0;
  this->pose.y = 0.0;
  this->pose.heading = 0.0;
  this->scale_factor=DEFAULT_SCALE_FACTOR;
}

COpendriveGeometry::COpendriveGeometry(const COpendriveGeometry &object)
{
  this->min_s = object.min_s;
  this->max_s = object.max_s;
  this->pose.x = object.pose.x;
  this->pose.y = object.pose.y;
  this->pose.heading = object.pose.heading;
  this->scale_factor = object.scale_factor;
}

void COpendriveGeometry::print(std::ostream& out)
{
  TOpendriveWorldPose tmp_pose=this->get_start_pose();

  out << "        Geometry " << this->get_name() << std::endl; 
  out << "          lenght: " << this->get_length() << std::endl;
  out << "          pose: x = " << tmp_pose.x << ", y = " << tmp_pose.y << ", heading = " << tmp_pose.heading << std::endl;
}

void COpendriveGeometry::load(const planView::geometry_type &geometry_info)
{
  this->min_s = (geometry_info.s().present() ? geometry_info.s().get() : 0.0);
  this->max_s = min_s + (geometry_info.length().present() ? geometry_info.length().get() : 0.0);
  this->pose.x = (geometry_info.x().present() ? geometry_info.x().get() : 0.0);
  this->pose.y = (geometry_info.y().present() ? geometry_info.y().get() : 0.0);
  this->pose.heading = (geometry_info.hdg().present() ? geometry_info.hdg().get() : 0.0);
  this->load_params(geometry_info);
}

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

void COpendriveGeometry::set_start_pose(TOpendriveWorldPose &pose)
{
  this->pose.x=pose.x*this->scale_factor;
  this->pose.y=pose.y*this->scale_factor;
  this->pose.heading=pose.heading;
}

void COpendriveGeometry::set_max_s(double s)
{
  this->max_s=s*this->scale_factor;
}

void COpendriveGeometry::set_min_s(double s)
{
  this->min_s=s*this->scale_factor;
}

bool COpendriveGeometry::get_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
{
  return this->transform_local_pose(track,local);
}

bool COpendriveGeometry::get_world_pose(const TOpendriveTrackPose &track,TOpendriveWorldPose &world) const
{
  TOpendriveLocalPose local;

  if(this->transform_local_pose(track,local))
  {
    world.heading=normalize_angle(this->pose.heading+local.heading);
    world.x=local.u*std::cos(this->pose.heading)-local.v*std::sin(this->pose.heading)+this->pose.x/this->scale_factor;
    world.y=local.u*std::sin(this->pose.heading)+local.v*std::cos(this->pose.heading)+this->pose.y/this->scale_factor;
    return true;
  }
  else
    return false;
}

bool COpendriveGeometry::in_range(double s) const
{
  if((s<(this->max_s/this->scale_factor)) && (s>=(this->min_s/this->scale_factor)))
    return true;
  else 
    return false;
}

double COpendriveGeometry::get_length(void) const
{
  return (this->max_s-this->min_s)/this->scale_factor;
}

double COpendriveGeometry::get_max_s(void) const
{
  return this->max_s/this->scale_factor;
}

double COpendriveGeometry::get_min_s(void) const
{
  return this->min_s/this->scale_factor;
}

TOpendriveWorldPose COpendriveGeometry::get_start_pose(void) const
{
  TOpendriveWorldPose tmp_pose;
  
  tmp_pose.x=this->pose.x/this->scale_factor;
  tmp_pose.y=this->pose.y/this->scale_factor;
  tmp_pose.heading=this->pose.heading;

  return tmp_pose;
}

TOpendriveWorldPose COpendriveGeometry::get_end_pose(void) const
{
  TOpendriveTrackPose track_pose;
  TOpendriveWorldPose world_pose;
 
  track_pose.s=this->get_length();
  track_pose.t=0.0;
  track_pose.heading=0.0;
  this->get_world_pose(track_pose,world_pose);

  return world_pose;
}

void COpendriveGeometry::operator=(const COpendriveGeometry &object)
{
  this->min_s = object.min_s;
  this->max_s = object.max_s;
  this->pose.x = object.pose.x;
  this->pose.y = object.pose.y;
  this->pose.heading = object.pose.heading;
  this->scale_factor = object.scale_factor;
}

std::ostream& operator<<(std::ostream& out, COpendriveGeometry &geom)
{
  geom.print(out);
  return out;
}

COpendriveGeometry::~COpendriveGeometry()
{
  this->min_s = 0.0;
  this->max_s = 0.0;
  this->pose.x = 0.0;
  this->pose.y = 0.0;
  this->pose.heading = 0.0;
  this->scale_factor = DEFAULT_SCALE_FACTOR;
}