opendrive_param_poly3.cpp 4.12 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
#include "opendrive_param_poly3.h"
#include <cmath>

COpendriveParamPoly3::COpendriveParamPoly3()
{
  this->u.a=0.0;  
  this->u.b=0.0;  
  this->u.c=0.0;  
  this->u.c=0.0;  
  this->v.a=0.0;  
  this->v.b=0.0;  
  this->v.c=0.0;  
  this->v.c=0.0;
  this->normalized=true;
}

COpendriveParamPoly3::COpendriveParamPoly3(const COpendriveParamPoly3 &object) : COpendriveGeometry(object)
{
  this->u.a=object.u.a;
  this->u.b=object.u.b;
  this->u.c=object.u.c;
  this->u.d=object.u.d;
  this->v.a=object.v.a;
  this->v.b=object.v.b;
  this->v.c=object.v.c;
  this->v.d=object.v.d;
  this->normalized=object.normalized;
}

30
bool COpendriveParamPoly3::transform_local_pose(const TOpendriveTrackPose &track,TOpendriveLocalPose &local) const
31
{
32
  double p = (this->normalized ? track.s/((this->max_s - this->min_s)/this->scale_factor):track.s);
33
34
35
36
37
38
39
40
41
42
43
44
45
46
  double p2 = p*p;
  double p3 = p2*p;
  double du = this->u.b + 2*this->u.c*p + 3*this->u.d*p2;
  double dv = this->v.b + 2*this->v.c*p + 3*this->v.d*p2;
  double alpha = std::atan2(dv, du);
  local.u = this->u.a + this->u.b*p + this->u.c*p2 + this->u.d*p3 - track.t*std::sin(alpha);
  local.v = this->v.a + this->v.b*p + this->v.c*p2 + this->v.d*p3 + track.t*std::cos(alpha);
  local.heading = normalize_angle(track.heading + alpha);

  return true;
}

void COpendriveParamPoly3::print(std::ostream &out)
{
47
  COpendriveGeometry::print(out);
48
49
  std::cout << "          U params: a = " << this->u.a << ", b = " << this->u.b  << ", c = " << this->u.c  << ", d = " << this->u.d << std::endl;
  std::cout << "          V params: a = " << this->v.a << ", b = " << this->v.b  << ", c = " << this->v.c  << ", d = " << this->v.d << std::endl;
50
  if(this->normalized)
51
    std::cout << "          Normalized" << std::endl;
52
  else
53
    std::cout << "          Not normalized" << std::endl;
54
55
56
57
58
}

std::string COpendriveParamPoly3::get_name(void)
{
  return std::string("Parametric polynomial 3th degree");
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
}

void COpendriveParamPoly3::load_params(const planView::geometry_type &geometry_info)
{
  this->u.a = (geometry_info.paramPoly3().get().aU().present() ? geometry_info.paramPoly3().get().aU().get() : 0.0);
  this->u.b = (geometry_info.paramPoly3().get().bU().present() ? geometry_info.paramPoly3().get().bU().get() : 0.0);
  this->u.c = (geometry_info.paramPoly3().get().cU().present() ? geometry_info.paramPoly3().get().cU().get() : 0.0);
  this->u.d = (geometry_info.paramPoly3().get().dU().present() ? geometry_info.paramPoly3().get().dU().get() : 0.0);
  this->v.a = (geometry_info.paramPoly3().get().aV().present() ? geometry_info.paramPoly3().get().aV().get() : 0.0);
  this->v.b = (geometry_info.paramPoly3().get().bV().present() ? geometry_info.paramPoly3().get().bV().get() : 0.0);
  this->v.c = (geometry_info.paramPoly3().get().cV().present() ? geometry_info.paramPoly3().get().cV().get() : 0.0);
  this->v.d = (geometry_info.paramPoly3().get().dV().present() ? geometry_info.paramPoly3().get().dV().get() : 0.0);
  this->normalized = true;
  if (geometry_info.paramPoly3().get().pRange().present() && geometry_info.paramPoly3().get().pRange().get() == pRange::arcLength)
    this->normalized = false;
}

COpendriveGeometry *COpendriveParamPoly3::clone(void)
{
  COpendriveParamPoly3 *new_poly=new COpendriveParamPoly3(*this);

  return new_poly;
}

83
84
85
86
87
88
void COpendriveParamPoly3::get_curvature(double &start,double &end)
{
  start=0.0;
  end=0.0;
}

89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
TOpendrivePoly3Params COpendriveParamPoly3::get_u_params(void)
{
  return this->u;
}

TOpendrivePoly3Params COpendriveParamPoly3::get_v_params(void)
{
  return this->v;
}

bool COpendriveParamPoly3::is_normalized(void)
{
  return this->normalized;
}

void COpendriveParamPoly3::operator=(const COpendriveParamPoly3 &object)
{
  COpendriveGeometry::operator=(object);
  this->u.a=object.u.a;
  this->u.b=object.u.b;
  this->u.c=object.u.c;
  this->u.d=object.u.d;
  this->v.a=object.v.a;
  this->v.b=object.v.b;
  this->v.c=object.v.c;
  this->v.d=object.v.d;
  this->normalized=object.normalized;
}

COpendriveParamPoly3::~COpendriveParamPoly3()
{
  this->u.a=0.0;  
  this->u.b=0.0;  
  this->u.c=0.0;  
  this->u.c=0.0;  
  this->v.a=0.0;  
  this->v.b=0.0;  
  this->v.c=0.0;  
  this->v.c=0.0;  
  this->normalized=true;
}