opendrive_arc.cpp 1.64 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
#include "opendrive_arc.h"
#include <cmath>

COpendriveArc::COpendriveArc()
{
  this->curvature=0.0;
}

COpendriveArc::COpendriveArc(const COpendriveArc &object) : COpendriveGeometry(object)
{
  this->curvature=object.curvature;
}

14
bool COpendriveArc::transform_local_pose(TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
15
16
17
{
  double alpha;

18
19
20
21
  alpha = track.s*this->curvature*this->scale_factor;
  local.u = std::sin(alpha)/(this->curvature*this->scale_factor) - track.t*std::sin(alpha);
  local.v = (1 - std::cos(alpha))/(this->curvature*this->scale_factor) + track.t*std::cos(alpha);
  local.heading = normalize_angle(track.heading + alpha);
22
23
24
25
26
  return true;
}

void COpendriveArc::print(std::ostream &out)
{
27
28
  double start_curvature,end_curvature;

29
  COpendriveGeometry::print(out);
30
31
  this->get_curvature(start_curvature,end_curvature);
  out << "          curvature = " << start_curvature << std::endl;
32
33
34
35
36
}

std::string COpendriveArc::get_name(void)
{
  return std::string("arc");
37
38
39
40
41
42
43
44
45
46
47
48
49
50
}

void COpendriveArc::load_params(const planView::geometry_type &geometry_info)
{
  this->curvature = (geometry_info.arc().get().curvature().present() ? geometry_info.arc().get().curvature().get() : 0.0);
}

COpendriveGeometry *COpendriveArc::clone(void)
{
  COpendriveArc *new_arc=new COpendriveArc(*this);

  return new_arc;
}

51
void COpendriveArc::get_curvature(double &start,double &end)
52
{
53
54
  start=this->curvature*this->scale_factor;
  end=this->curvature*this->scale_factor;
55
56
57
58
59
60
61
62
63
64
65
66
67
}

void COpendriveArc::operator=(const COpendriveArc &object)
{
  COpendriveGeometry::operator=(object);
  this->curvature=object.curvature;
}

COpendriveArc::~COpendriveArc()
{
  this->curvature=0.0;
}