CARLA
Geometry.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/geom/Location.h"
10 #include "carla/geom/Math.h"
12 #include "carla/geom/Rtree.h"
13 
14 namespace carla {
15 namespace road {
16 namespace element {
17 
18  enum class GeometryType : unsigned int {
19  LINE,
20  ARC,
21  SPIRAL,
22  POLY3,
24  };
25 
26  struct DirectedPoint {
27 
29  : location(0, 0, 0),
30  tangent(0) {}
31  DirectedPoint(const geom::Location &l, double t)
32  : location(l),
33  tangent(t) {}
34  DirectedPoint(float x, float y, float z, double t)
35  : location(x, y, z),
36  tangent(t) {}
37 
38  geom::Location location = {0.0f, 0.0f, 0.0f};
39  double tangent = 0.0; // [radians]
40  double pitch = 0.0; // [radians]
41 
42  void ApplyLateralOffset(float lateral_offset);
43 
44  friend bool operator==(const DirectedPoint &lhs, const DirectedPoint &rhs) {
45  return (lhs.location == rhs.location) && (lhs.tangent == rhs.tangent);
46  }
47  };
48 
49  class Geometry {
50  public:
51 
53  return _type;
54  }
55  double GetLength() const {
56  return _length;
57  }
58  double GetStartOffset() const {
59  return _start_position_offset;
60  }
61  double GetHeading() const {
62  return _heading;
63  }
64 
66  return _start_position;
67  }
68 
69  virtual ~Geometry() = default;
70 
71  virtual DirectedPoint PosFromDist(double dist) const = 0;
72 
73  virtual std::pair<float, float> DistanceTo(const geom::Location &p) const = 0;
74 
75  protected:
76 
78  GeometryType type,
79  double start_offset,
80  double length,
81  double heading,
82  const geom::Location &start_pos)
83  : _type(type),
84  _length(length),
85  _start_position_offset(start_offset),
86  _heading(heading),
87  _start_position(start_pos) {}
88 
89  protected:
90 
91  GeometryType _type; // geometry type
92  double _length; // length of the road section [meters]
93 
94  double _start_position_offset; // s-offset [meters]
95  double _heading; // start orientation [radians]
96 
98  };
99 
100  class GeometryLine final : public Geometry {
101  public:
102 
104  double start_offset,
105  double length,
106  double heading,
107  const geom::Location &start_pos)
108  : Geometry(GeometryType::LINE, start_offset, length, heading, start_pos) {}
109 
110  DirectedPoint PosFromDist(double dist) const override;
111 
112  /// Returns a pair containing:
113  /// - @b first: distance to the nearest point in this line from the
114  /// beginning of the shape.
115  /// - @b second: Euclidean distance from the nearest point in this line to
116  /// p.
117  /// @param p point to calculate the distance
118  std::pair<float, float> DistanceTo(const geom::Location &p) const override {
120  p,
121  _start_position,
122  PosFromDist(_length).location);
123  }
124 
125  };
126 
127  class GeometryArc final : public Geometry {
128  public:
129 
131  double start_offset,
132  double length,
133  double heading,
134  const geom::Location &start_pos,
135  double curv)
136  : Geometry(GeometryType::ARC, start_offset, length, heading, start_pos),
137  _curvature(curv) {}
138 
139  DirectedPoint PosFromDist(double dist) const override;
140 
141  /// Returns a pair containing:
142  /// - @b first: distance to the nearest point in this arc from the
143  /// beginning of the shape.
144  /// - @b second: Euclidean distance from the nearest point in this arc to p.
145  /// @param p point to calculate the distance
146  std::pair<float, float> DistanceTo(const geom::Location &p) const override {
148  p,
149  _start_position,
150  static_cast<float>(_length),
151  static_cast<float>(_heading),
152  static_cast<float>(_curvature));
153  }
154 
155  double GetCurvature() const {
156  return _curvature;
157  }
158 
159  private:
160 
161  double _curvature;
162  };
163 
164  class GeometrySpiral final : public Geometry {
165  public:
166 
168  double start_offset,
169  double length,
170  double heading,
171  const geom::Location &start_pos,
172  double curv_s,
173  double curv_e)
174  : Geometry(GeometryType::SPIRAL, start_offset, length, heading, start_pos),
175  _curve_start(curv_s),
176  _curve_end(curv_e) {}
177 
178  double GetCurveStart() {
179  return _curve_start;
180  }
181 
182  double GetCurveEnd() {
183  return _curve_end;
184  }
185 
186  DirectedPoint PosFromDist(double dist) const override;
187 
188  std::pair<float, float> DistanceTo(const geom::Location &) const override;
189 
190  private:
191 
192  double _curve_start;
193  double _curve_end;
194  };
195 
196  class GeometryPoly3 final : public Geometry {
197  public:
198 
200  double start_offset,
201  double length,
202  double heading,
203  const geom::Location &start_pos,
204  double a,
205  double b,
206  double c,
207  double d)
208  : Geometry(GeometryType::POLY3, start_offset, length, heading, start_pos),
209  _a(a),
210  _b(b),
211  _c(c),
212  _d(d) {
213  _poly.Set(a, b, c, d);
214  PreComputeSpline();
215  }
216 
217  double Geta() const {
218  return _a;
219  }
220  double Getb() const {
221  return _b;
222  }
223  double Getc() const {
224  return _c;
225  }
226  double Getd() const {
227  return _d;
228  }
229 
230  DirectedPoint PosFromDist(double dist) const override;
231 
232  std::pair<float, float> DistanceTo(const geom::Location &) const override;
233 
234  private:
235 
237 
238  double _a;
239  double _b;
240  double _c;
241  double _d;
242 
243  struct RtreeValue {
244  double u = 0;
245  double v = 0;
246  double s = 0;
247  double t = 0;
248  };
252  void PreComputeSpline();
253  };
254 
255  class GeometryParamPoly3 final : public Geometry {
256  public:
257 
259  double start_offset,
260  double length,
261  double heading,
262  const geom::Location &start_pos,
263  double aU,
264  double bU,
265  double cU,
266  double dU,
267  double aV,
268  double bV,
269  double cV,
270  double dV,
271  bool arcLength)
272  : Geometry(GeometryType::POLY3PARAM, start_offset, length, heading, start_pos),
273  _aU(aU),
274  _bU(bU),
275  _cU(cU),
276  _dU(dU),
277  _aV(aV),
278  _bV(bV),
279  _cV(cV),
280  _dV(dV),
281  _arcLength(arcLength) {
282  _polyU.Set(aU, bU, cU, dU);
283  _polyV.Set(aV, bV, cV, dV);
284  PreComputeSpline();
285  }
286 
287  double GetaU() const {
288  return _aU;
289  }
290  double GetbU() const {
291  return _bU;
292  }
293  double GetcU() const {
294  return _cU;
295  }
296  double GetdU() const {
297  return _dU;
298  }
299  double GetaV() const {
300  return _aV;
301  }
302  double GetbV() const {
303  return _bV;
304  }
305  double GetcV() const {
306  return _cV;
307  }
308  double GetdV() const {
309  return _dV;
310  }
311 
312  DirectedPoint PosFromDist(double dist) const override;
313 
314  std::pair<float, float> DistanceTo(const geom::Location &) const override;
315 
316  private:
317 
320  double _aU;
321  double _bU;
322  double _cU;
323  double _dU;
324  double _aV;
325  double _bV;
326  double _cV;
327  double _dV;
329 
330  struct RtreeValue {
331  double u = 0;
332  double v = 0;
333  double s = 0;
334  double t_u = 0;
335  double t_v = 0;
336  };
340  void PreComputeSpline();
341  };
342 
343 } // namespace element
344 } // namespace road
345 } // namespace carla
double GetStartOffset() const
Definition: Geometry.h:58
std::pair< float, float > DistanceTo(const geom::Location &p) const override
Returns a pair containing:
Definition: Geometry.h:146
Describes a Cubic Polynomial so: f(x) = a + bx + cx^2 + dx^3.
double GetHeading() const
Definition: Geometry.h:61
static std::pair< float, float > DistanceSegmentToPoint(const Vector3D &p, const Vector3D &v, const Vector3D &w)
Returns a pair containing:
Definition: Math.cpp:18
geom::Location _start_position
Definition: Geometry.h:97
DirectedPoint(float x, float y, float z, double t)
Definition: Geometry.h:34
GeometryType GetType() const
Definition: Geometry.h:52
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
const geom::Location & GetStartPosition()
Definition: Geometry.h:65
GeometrySpiral(double start_offset, double length, double heading, const geom::Location &start_pos, double curv_s, double curv_e)
Definition: Geometry.h:167
GeometryPoly3(double start_offset, double length, double heading, const geom::Location &start_pos, double a, double b, double c, double d)
Definition: Geometry.h:199
static std::pair< float, float > DistanceArcToPoint(Vector3D p, Vector3D start_pos, float length, float heading, float curvature)
Returns a pair containing:
Definition: Math.cpp:33
DirectedPoint(const geom::Location &l, double t)
Definition: Geometry.h:31
GeometryLine(double start_offset, double length, double heading, const geom::Location &start_pos)
Definition: Geometry.h:103
Geometry(GeometryType type, double start_offset, double length, double heading, const geom::Location &start_pos)
Definition: Geometry.h:77
double GetLength() const
Definition: Geometry.h:55
GeometryParamPoly3(double start_offset, double length, double heading, const geom::Location &start_pos, double aU, double bU, double cU, double dU, double aV, double bV, double cV, double dV, bool arcLength)
Definition: Geometry.h:258
geom::CubicPolynomial _poly
Definition: Geometry.h:236
GeometryArc(double start_offset, double length, double heading, const geom::Location &start_pos, double curv)
Definition: Geometry.h:130
std::pair< float, float > DistanceTo(const geom::Location &p) const override
Returns a pair containing:
Definition: Geometry.h:118
Rtree::TreeElement TreeElement
Definition: Geometry.h:250
friend bool operator==(const DirectedPoint &lhs, const DirectedPoint &rhs)
Definition: Geometry.h:44
std::pair< BSegment, std::pair< RtreeValue, RtreeValue > > TreeElement
Definition: Rtree.h:83