CARLA
CubicPolynomial.h
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #pragma once
8 
9 #include "carla/Debug.h"
10 
11 #include <array>
12 
13 namespace carla {
14 namespace geom {
15 
16  /// Describes a Cubic Polynomial so:
17  /// f(x) = a + bx + cx^2 + dx^3
19  public:
20 
21  using value_type = double;
22 
23  // =========================================================================
24  // -- Constructors ---------------------------------------------------------
25  // =========================================================================
26 
27  CubicPolynomial() = default;
28 
29  CubicPolynomial(const CubicPolynomial &) = default;
30 
32  const value_type &a,
33  const value_type &b,
34  const value_type &c,
35  const value_type &d)
36  : _v{ {a, b, c, d} },
37  _s(0.0) {}
38 
40  const value_type &a,
41  const value_type &b,
42  const value_type &c,
43  const value_type &d,
44  const value_type &s) // lateral offset
45  : _v{ {a - b * s + c * s * s - d * s * s * s,
46  b - 2 * c * s + 3 * d * s * s,
47  c - 3 * d * s,
48  d} },
49  _s(s) {}
50 
51  // =========================================================================
52  // -- Getters --------------------------------------------------------------
53  // =========================================================================
54 
55  value_type GetA() const {
56  return _v[0];
57  }
58 
59  value_type GetB() const {
60  return _v[1];
61  }
62 
63  value_type GetC() const {
64  return _v[2];
65  }
66 
67  value_type GetD() const {
68  return _v[3];
69  }
70 
71  value_type GetS() const {
72  return _s;
73  }
74 
75  // =========================================================================
76  // -- Set ------------------------------------------------------------------
77  // =========================================================================
78 
79  void Set(
80  const value_type &a,
81  const value_type &b,
82  const value_type &c,
83  const value_type &d,
84  const value_type &s) { // lateral offset
85  _v = { a - b * s + c * s * s - d * s * s * s,
86  b - 2 * c * s + 3 * d * s * s,
87  c - 3 * d * s,
88  d };
89  _s = s;
90  }
91 
92  void Set(
93  const value_type &a,
94  const value_type &b,
95  const value_type &c,
96  const value_type &d) {
97  _v = {a, b, c, d};
98  _s = 0.0;
99  }
100 
101  // =========================================================================
102  // -- Evaluate methods -----------------------------------------------------
103  // =========================================================================
104 
105  /// Evaluates f(x) = a + bx + cx^2 + dx^3
106  value_type Evaluate(const value_type &x) const {
107  // return _v[0] + _v[1] * (x) + _v[2] * (x * x) + _v[3] * (x * x * x);
108  return _v[0] + x * (_v[1] + x * (_v[2] + x * _v[3]));
109  }
110 
111  /// Evaluates the tangent using df/dx = b + 2cx + 3dx^2
112  value_type Tangent(const value_type &x) const {
113  return _v[1] + x * (2 * _v[2] + x * 3 * _v[3]);
114  }
115 
116  // =========================================================================
117  // -- Arithmetic operators -------------------------------------------------
118  // =========================================================================
119 
121  for (auto i = 0u; i < _v.size(); ++i) {
122  _v[i] += rhs._v[i];
123  }
124  return *this;
125  }
126 
128  lhs += rhs;
129  return lhs;
130  }
131 
133  for (auto i = 0u; i < _v.size(); ++i) {
134  _v[i] *= rhs;
135  }
136  return *this;
137  }
138 
140  lhs *= rhs;
141  return lhs;
142  }
143 
145  rhs *= lhs;
146  return rhs;
147  }
148 
149  private:
150 
151  // =========================================================================
152  // -- Private data members -------------------------------------------------
153  // =========================================================================
154 
155  // a - elevation
156  // b - slope
157  // c - vertical curvature
158  // d - curvature change
159  std::array<value_type, 4> _v = {0.0, 0.0, 0.0, 0.0};
160 
161  // s - distance
163  };
164 
165 } // namespace geom
166 } // namespace carla
void Set(const value_type &a, const value_type &b, const value_type &c, const value_type &d)
Describes a Cubic Polynomial so: f(x) = a + bx + cx^2 + dx^3.
void Set(const value_type &a, const value_type &b, const value_type &c, const value_type &d, const value_type &s)
friend CubicPolynomial operator*(const value_type &lhs, CubicPolynomial rhs)
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
CubicPolynomial(const value_type &a, const value_type &b, const value_type &c, const value_type &d)
value_type Evaluate(const value_type &x) const
Evaluates f(x) = a + bx + cx^2 + dx^3.
friend CubicPolynomial operator*(CubicPolynomial lhs, const value_type &rhs)
value_type Tangent(const value_type &x) const
Evaluates the tangent using df/dx = b + 2cx + 3dx^2.
CubicPolynomial & operator+=(const CubicPolynomial &rhs)
CubicPolynomial & operator*=(const value_type &rhs)
CubicPolynomial(const value_type &a, const value_type &b, const value_type &c, const value_type &d, const value_type &s)
std::array< value_type, 4 > _v
friend CubicPolynomial operator+(CubicPolynomial lhs, const CubicPolynomial &rhs)