CARLA
Geometry.cpp
Go to the documentation of this file.
1 // Copyright (c) 2019 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 
8 
9 #include "carla/Debug.h"
10 #include "carla/Exception.h"
11 #include "carla/geom/Location.h"
12 #include "carla/geom/Math.h"
13 #include "carla/geom/Vector2D.h"
14 
15 #include <boost/array.hpp>
16 #include <boost/math/tools/rational.hpp>
17 
18 #include <odrSpiral/odrSpiral.h>
19 
20 #include <algorithm>
21 #include <cmath>
22 #include <stdexcept>
23 
24 namespace carla {
25 namespace road {
26 namespace element {
27 
28  void DirectedPoint::ApplyLateralOffset(float lateral_offset) {
29  /// @todo Z axis??
30  auto normal_x = std::sin(static_cast<float>(tangent));
31  auto normal_y = -std::cos(static_cast<float>(tangent));
32  location.x += lateral_offset * normal_x;
33  location.y += lateral_offset * normal_y;
34  }
35 
37  DEBUG_ASSERT(_length > 0.0);
38  dist = geom::Math::Clamp(dist, 0.0, _length);
39  DirectedPoint p(_start_position, _heading);
40  p.location.x += static_cast<float>(dist * std::cos(p.tangent));
41  p.location.y += static_cast<float>(dist * std::sin(p.tangent));
42  return p;
43  }
44 
46  dist = geom::Math::Clamp(dist, 0.0, _length);
47  DEBUG_ASSERT(_length > 0.0);
48  DEBUG_ASSERT(std::fabs(_curvature) > 1e-15);
49  const double radius = 1.0 / _curvature;
50  constexpr double pi_half = geom::Math::Pi<double>() / 2.0;
51  DirectedPoint p(_start_position, _heading);
52  p.location.x += static_cast<float>(radius * std::cos(p.tangent + pi_half));
53  p.location.y += static_cast<float>(radius * std::sin(p.tangent + pi_half));
54  p.tangent += dist * _curvature;
55  p.location.x -= static_cast<float>(radius * std::cos(p.tangent + pi_half));
56  p.location.y -= static_cast<float>(radius * std::sin(p.tangent + pi_half));
57  return p;
58  }
59 
60  // helper function for rotating points
61  geom::Vector2D RotatebyAngle(double angle, double x, double y) {
62  const double cos_a = std::cos(angle);
63  const double sin_a = std::sin(angle);
64  return geom::Vector2D(
65  static_cast<float>(x * cos_a - y * sin_a),
66  static_cast<float>(y * cos_a + x * sin_a));
67  }
68 
70  dist = geom::Math::Clamp(dist, 0.0, _length);
71  DEBUG_ASSERT(_length > 0.0);
72  DirectedPoint p(_start_position, _heading);
73 
74  const double curve_end = (_curve_end);
75  const double curve_start = (_curve_start);
76  const double curve_dot = (curve_end - curve_start) / (_length);
77  const double s_o = curve_start / curve_dot;
78  double s = s_o + dist;
79 
80  double x;
81  double y;
82  double t;
83  odrSpiral(s, curve_dot, &x, &y, &t);
84 
85  double x_o;
86  double y_o;
87  double t_o;
88  odrSpiral(s_o, curve_dot, &x_o, &y_o, &t_o);
89 
90  x = x - x_o;
91  y = y - y_o;
92  t = t - t_o;
93 
94  geom::Vector2D pos = RotatebyAngle(_heading - t_o, x, y);
95  p.location.x += pos.x;
96  p.location.y += pos.y;
97  p.tangent = _heading + t;
98 
99  return p;
100  }
101 
102  /// @todo
103  std::pair<float, float> GeometrySpiral::DistanceTo(const geom::Location &location) const {
104  // Not analytic, discretize and find nearest point
105  // throw_exception(std::runtime_error("not implemented"));
106  return {location.x - _start_position.x, location.y - _start_position.y};
107  }
108 
110  auto result = _rtree.GetNearestNeighbours(
111  Rtree::BPoint(static_cast<float>(dist))).front();
112 
113  auto &val1 = result.second.first;
114  auto &val2 = result.second.second;
115 
116  double rate = (val2.s - dist) / (val2.s - val1.s);
117  double u = rate * val1.u + (1.0 - rate) * val2.u;
118  double v = rate * val1.v + (1.0 - rate) * val2.v;
119  double tangent = atan((rate * val1.t + (1.0 - rate) * val2.t)); // ?
120 
121  geom::Vector2D pos = RotatebyAngle(_heading, u, v);
122  DirectedPoint p(_start_position, _heading + tangent);
123  p.location.x += pos.x;
124  p.location.y += pos.y;
125  return p;
126  }
127 
128  std::pair<float, float> GeometryPoly3::DistanceTo(const geom::Location & /*p*/) const {
129  // No analytical expression (Newton-Raphson?/point search)
130  // throw_exception(std::runtime_error("not implemented"));
131  return {_start_position.x, _start_position.y};
132  }
133 
135  // Roughly the interval size in m
136  constexpr double interval_size = 0.3;
137  const double delta_u = interval_size; // interval between values of u
138  double current_s = 0;
139  double current_u = 0;
140  double last_u = 0;
141  double last_v = _poly.Evaluate(current_u);
142  double last_s = 0;
143  RtreeValue last_val{last_u, last_v, last_s, _poly.Tangent(current_u)};
144  while (current_s < _length + delta_u) {
145  current_u += delta_u;
146  double current_v = _poly.Evaluate(current_u);
147  double du = current_u - last_u;
148  double dv = current_v - last_v;
149  double ds = sqrt(du * du + dv * dv);
150  current_s += ds;
151  double current_t = _poly.Tangent(current_u);
152  RtreeValue current_val{current_u, current_v, current_s, current_t};
153 
154  Rtree::BPoint p1(static_cast<float>(last_s));
155  Rtree::BPoint p2(static_cast<float>(current_s));
156  _rtree.InsertElement(Rtree::BSegment(p1, p2), last_val, current_val);
157 
158  last_u = current_u;
159  last_v = current_v;
160  last_s = current_s;
161  last_val = current_val;
162 
163  }
164  }
165 
167  auto result = _rtree.GetNearestNeighbours(
168  Rtree::BPoint(static_cast<float>(dist))).front();
169 
170  auto &val1 = result.second.first;
171  auto &val2 = result.second.second;
172  double rate = (val2.s - dist) / (val2.s - val1.s);
173  double u = rate * val1.u + (1.0 - rate) * val2.u;
174  double v = rate * val1.v + (1.0 - rate) * val2.v;
175  double t_u = (rate * val1.t_u + (1.0 - rate) * val2.t_u);
176  double t_v = (rate * val1.t_v + (1.0 - rate) * val2.t_v);
177  double tangent = atan2(t_v, t_u); // ?
178 
179  geom::Vector2D pos = RotatebyAngle(_heading, u, v);
180  DirectedPoint p(_start_position, _heading + tangent);
181  p.location.x += pos.x;
182  p.location.y += pos.y;
183  return p;
184  }
185  std::pair<float, float> GeometryParamPoly3::DistanceTo(const geom::Location &) const {
186  // No analytical expression (Newton-Raphson?/point search)
187  // throw_exception(std::runtime_error("not implemented"));
188  return {_start_position.x, _start_position.y};
189  }
190 
192  // Roughly the interval size in m
193  constexpr double interval_size = 0.5;
194  size_t number_intervals =
195  std::max(static_cast<size_t>(_length / interval_size), size_t(5));
196  double delta_p = 1.0 / number_intervals;
197  if (_arcLength) {
198  delta_p *= _length;
199  }
200  double param_p = 0;
201  double current_s = 0;
202  double last_u = _polyU.Evaluate(param_p);
203  double last_v = _polyV.Evaluate(param_p);
204  double last_s = 0;
205  RtreeValue last_val{
206  last_u,
207  last_v,
208  last_s,
209  _polyU.Tangent(param_p),
210  _polyV.Tangent(param_p) };
211  for(size_t i = 0; i < number_intervals; ++i) {
212  param_p += delta_p;
213  double current_u = _polyU.Evaluate(param_p);
214  double current_v = _polyV.Evaluate(param_p);
215  double du = current_u - last_u;
216  double dv = current_v - last_v;
217  double ds = sqrt(du * du + dv * dv);
218  current_s += ds;
219  double current_t_u = _polyU.Tangent(param_p);
220  double current_t_v = _polyV.Tangent(param_p);
221  RtreeValue current_val{
222  current_u,
223  current_v,
224  current_s,
225  current_t_u,
226  current_t_v };
227 
228  Rtree::BPoint p1(static_cast<float>(last_s));
229  Rtree::BPoint p2(static_cast<float>(current_s));
230  _rtree.InsertElement(Rtree::BSegment(p1, p2), last_val, current_val);
231 
232  last_u = current_u;
233  last_v = current_v;
234  last_s = current_s;
235  last_val = current_val;
236 
237  if(current_s > _length){
238  break;
239  }
240  }
241  }
242 } // namespace element
243 } // namespace road
244 } // namespace carla
DirectedPoint PosFromDist(double dist) const override
Definition: Geometry.cpp:69
geom::Vector2D RotatebyAngle(double angle, double x, double y)
Definition: Geometry.cpp:61
static T Clamp(T a, T min=T(0), T max=T(1))
Definition: Math.h:49
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
DirectedPoint PosFromDist(double dist) const override
Definition: Geometry.cpp:166
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
boost::geometry::model::point< float, Dimension, boost::geometry::cs::cartesian > BPoint
Definition: Rtree.h:81
DirectedPoint PosFromDist(double dist) const override
Definition: Geometry.cpp:36
DirectedPoint PosFromDist(double dist) const override
Definition: Geometry.cpp:45
std::pair< float, float > DistanceTo(const geom::Location &) const override
Definition: Geometry.cpp:185
void odrSpiral(double s, double cDot, double *x, double *y, double *t)
compute the actual "standard" spiral, starting with curvature 0
Definition: odrSpiral.cpp:234
DirectedPoint PosFromDist(double dist) const override
Definition: Geometry.cpp:109
boost::geometry::model::segment< BPoint > BSegment
Definition: Rtree.h:82
std::pair< float, float > DistanceTo(const geom::Location &) const override
Definition: Geometry.cpp:103
geom::Vector2D Vector2D
Definition: rpc/Vector2D.h:14
void ApplyLateralOffset(float lateral_offset)
Definition: Geometry.cpp:28
std::pair< float, float > DistanceTo(const geom::Location &) const override
Definition: Geometry.cpp:128