Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2020-2022 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : /**
10 : : * @file
11 : : */
12 : :
13 : : #pragma once
14 : :
15 : : #include <boost/geometry.hpp>
16 : : #include <boost/geometry/geometries/geometries.hpp>
17 : : #include <boost/geometry/geometries/point_xy.hpp>
18 : : #include <boost/geometry/geometries/polygon.hpp>
19 : : #include <sstream>
20 : : #include "ad/physics/AngleOperation.hpp"
21 : : #include "ad/physics/Distance.hpp"
22 : : #include "ad/rss/state/HeadingRange.hpp"
23 : : #include "ad/rss/world/UnstructuredTrajectorySet.hpp"
24 : :
25 : : /*!
26 : : * @brief namespace ad
27 : : */
28 : : namespace ad {
29 : : /*!
30 : : * @brief namespace rss
31 : : */
32 : : namespace rss {
33 : : /*!
34 : : * @brief namespace unstructured
35 : : */
36 : : namespace unstructured {
37 : :
38 : : // Basic types
39 : : typedef boost::geometry::model::d2::point_xy<double> Point;
40 : : typedef boost::geometry::model::linestring<Point> Line;
41 : : typedef boost::geometry::model::polygon<Point, false> Polygon; // counterclockwise
42 : : typedef boost::geometry::model::multi_point<Point> MultiPoint;
43 : :
44 : : /**
45 : : * @brief create a Point from a Distance2D
46 : : *
47 : : * @param[in] distance distance to convert
48 : : *
49 : : * @returns point
50 : : */
51 : 3815 : inline Point toPoint(ad::physics::Distance2D const &distance)
52 : : {
53 [ + - ]: 3815 : return Point(static_cast<double>(distance.x), static_cast<double>(distance.y));
54 : : }
55 : :
56 : : /**
57 : : * @brief create a Point from two Distance components
58 : : *
59 : : * @param[in] distanceX x component of distance
60 : : * @param[in] distanceY y component of distance
61 : : *
62 : : * @returns point
63 : : */
64 : 7467 : inline Point toPoint(ad::physics::Distance const &distanceX, ad::physics::Distance const &distanceY)
65 : : {
66 [ + - ]: 7467 : return Point(static_cast<double>(distanceX), static_cast<double>(distanceY));
67 : : }
68 : :
69 : : /**
70 : : * @brief convert a Point to a Distance2D
71 : : *
72 : : * @param[in] point point to convert
73 : : *
74 : : * @returns distance
75 : : */
76 : 996 : inline ad::physics::Distance2D toDistance(Point const &point)
77 : : {
78 [ # # ]: 996 : ad::physics::Distance2D distance;
79 [ + - ]: 996 : distance.x = ad::physics::Distance(point.x());
80 [ + - ]: 996 : distance.y = ad::physics::Distance(point.y());
81 : 996 : return distance;
82 : : }
83 : :
84 : : /**
85 : : * @brief convert a trajectory set to a polygon
86 : : *
87 : : * @param[in] trajectorySet trajectory set to convert
88 : : * @param[out] polygon converted polygon
89 : : */
90 : : void toPolygon(world::UnstructuredTrajectorySet const &trajectorySet, Polygon &polygon);
91 : :
92 : : /**
93 : : * @brief convert a polygon to a trajectory set
94 : : *
95 : : * @param[in] polygon polygon to convert
96 : : * @param[out] trajectorySet converted trajectory set
97 : : */
98 : : void toTrajectorySet(Polygon const &polygon, world::UnstructuredTrajectorySet &trajectorySet);
99 : :
100 : : /**
101 : : * @brief check if an angle is within a range
102 : : *
103 : : * @param[in] angle angle to check
104 : : * @param[in] range heading range
105 : : *
106 : : * @returns true if inside angle range, otherwise false
107 : : */
108 : : bool isInsideHeadingRange(ad::physics::Angle const &angle, state::HeadingRange const &range);
109 : :
110 : : /**
111 : : * @brief get the overlap between two angle ranges
112 : : *
113 : : * @param[in] a first heading range
114 : : * @param[in] b second heading range
115 : : * @param[out] overlapRange overlapping heading ranges
116 : : *
117 : : * @returns true if overlap exists, otherwise false
118 : : */
119 : : bool getHeadingOverlap(state::HeadingRange const &a,
120 : : state::HeadingRange const &b,
121 : : std::vector<state::HeadingRange> &overlapRanges);
122 : :
123 : : /**
124 : : * @brief get the overlap between an angle range and a heading range
125 : : *
126 : : * @param[in] headingRange angle range
127 : : * @param[inout] overlapRanges overlapping heading ranges
128 : : *
129 : : * @returns true if overlap exists, otherwise false
130 : : */
131 : : bool getHeadingOverlap(state::HeadingRange const &headingRange, std::vector<state::HeadingRange> &overlapRanges);
132 : :
133 : : /**
134 : : * @brief rotate a point around another point
135 : : *
136 : : * @param[in] origin absolute origin
137 : : * @param[in] relativePoint point to rotate, relative to origin
138 : : * @param[in] angle angle to rotate the point
139 : : *
140 : : * @returns rotated point
141 : : */
142 : : Point rotateAroundPoint(Point const &origin, Point const &relativePoint, ad::physics::Angle const &angle);
143 : :
144 : : /**
145 : : * @brief calculate a point on a circle
146 : : *
147 : : * @param[in] origin absolute origin
148 : : * @param[in] radius radius of circle
149 : : * @param[in] angle angle to rotate the point
150 : : *
151 : : * @returns resulting point
152 : : */
153 : : Point getPointOnCircle(Point const &origin, ad::physics::Distance const &radius, ad::physics::Angle const &angle);
154 : :
155 : : /**
156 : : * @brief calculate the circle origin
157 : : *
158 : : * @param[in] point point on circle
159 : : * @param[in] radius radius of circle
160 : : * @param[in] angle circle angle for point
161 : : *
162 : : * @returns circle origin
163 : : */
164 : : Point getCircleOrigin(Point const &point, ad::physics::Distance const &radius, ad::physics::Angle const &angle);
165 : :
166 : : /**
167 : : * @brief calculate points on a circle arc
168 : : *
169 : : * @param[in] origin absolute origin
170 : : * @param[in] radius radius of circle
171 : : * @param[in] from starting angle
172 : : * @param[in] delta angle of the arc
173 : : * @param[out] geometry geometry the calculated points are added to
174 : : */
175 : : template <typename T>
176 : 1 : void calculateCircleArc(Point origin,
177 : : ad::physics::Distance const &radius,
178 : : ad::physics::Angle const &from,
179 : : ad::physics::Angle const &delta,
180 : : ad::physics::Angle const &stepWidth,
181 : : T &geometry)
182 : : {
183 : 1 : ad::physics::Angle currentAngle = from;
184 [ + - ]: 1 : ad::physics::Angle maxAngle = currentAngle + delta;
185 [ + - + + ]: 5 : while (currentAngle <= maxAngle)
186 : : {
187 [ + - + - ]: 4 : boost::geometry::append(geometry, getPointOnCircle(origin, radius, currentAngle));
188 [ + - ]: 4 : currentAngle += stepWidth;
189 : : }
190 [ + - + - : 1 : if (currentAngle - stepWidth != maxAngle)
- + ]
191 : : {
192 [ # # # # ]: 0 : boost::geometry::append(geometry, getPointOnCircle(origin, radius, maxAngle));
193 : : }
194 : 1 : }
195 : :
196 : : /**
197 : : * @brief check if two trajectory sets collide
198 : : *
199 : : * @param[in] trajectorySet1 first trajectory set
200 : : * @param[in] trajectorySet2 second trajectory set
201 : : *
202 : : * @returns true if trajectory sets collide, otherwise false
203 : : */
204 : : bool collides(world::UnstructuredTrajectorySet const &trajectorySet1,
205 : : world::UnstructuredTrajectorySet const &trajectorySet2);
206 : :
207 : : /**
208 : : * @brief Combine two polygons
209 : : *
210 : : * @param[in] a polygon a
211 : : * @param[in] b polygon b
212 : : * @param[out] result resulting polygon
213 : : *
214 : : * @returns false if a failure occurred during calculations, true otherwise
215 : : */
216 : : bool combinePolygon(Polygon const &a, Polygon const &b, Polygon &result);
217 : :
218 : : } // namespace unstructured
219 : : } // namespace rss
220 : : } // namespace ad
221 : :
222 : : /*!
223 : : * @brief Point operation: vector addition
224 : : *
225 : : * @param[in] a point a
226 : : * @param[in] b point b
227 : : *
228 : : * @returns c = a + b
229 : : */
230 : 5 : inline ad::rss::unstructured::Point operator+(ad::rss::unstructured::Point const &a,
231 : : ad::rss::unstructured::Point const &b)
232 : : {
233 : 5 : auto result = a;
234 [ + - ]: 5 : boost::geometry::add_point(result, b);
235 : 5 : return result;
236 : : }
237 : :
238 : : /*!
239 : : * @brief Point operation: vector subtraction
240 : : *
241 : : * @param[in] a point a
242 : : * @param[in] b point b
243 : : *
244 : : * @returns c = a - b
245 : : */
246 : 8853 : inline ad::rss::unstructured::Point operator-(ad::rss::unstructured::Point const &a,
247 : : ad::rss::unstructured::Point const &b)
248 : : {
249 : 8853 : auto result = a;
250 [ + - ]: 8853 : boost::geometry::subtract_point(result, b);
251 : 8853 : return result;
252 : : }
253 : :
254 : : /*!
255 : : * @brief comparison operation: Point
256 : : *
257 : : * @param[in] a point a
258 : : * @param[in] b point b
259 : : *
260 : : * @returns a == b
261 : : */
262 : 540 : inline bool operator==(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
263 : : {
264 : 540 : return (std::abs(a.x() - b.x()) <= std::numeric_limits<double>::epsilon())
265 [ + + + - ]: 540 : && (std::abs(a.y() - b.y()) <= std::numeric_limits<double>::epsilon());
266 : : }
267 : :
268 : : /*!
269 : : * @brief comparison operation: Points not equal
270 : : *
271 : : * @param[in] a point a
272 : : * @param[in] b point b
273 : : *
274 : : * @returns a != b
275 : : */
276 : : inline bool operator!=(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
277 : : {
278 : : return !(a == b);
279 : : }
280 : :
281 : : namespace std {
282 : :
283 : : /*!
284 : : * @brief to_string overload for Point
285 : : *
286 : : * @param[in] value a point
287 : : *
288 : : * @returns string describing point
289 : : */
290 : 0 : inline std::string to_string(ad::rss::unstructured::Point const &value)
291 : : {
292 [ # # ]: 0 : std::stringstream stream;
293 [ # # # # : 0 : stream << "[" << value.x() << "," << value.y() << "]";
# # # # #
# # # #
# ]
294 [ # # ]: 0 : return stream.str();
295 : : }
296 : :
297 : : /*!
298 : : * @brief to_string overload for Polygon
299 : : *
300 : : * @param[in] value a polygon
301 : : *
302 : : * @returns string describing polygon
303 : : */
304 : 0 : inline std::string to_string(ad::rss::unstructured::Polygon const &value)
305 : : {
306 [ # # ]: 0 : std::stringstream stream;
307 [ # # ]: 0 : stream << "[";
308 [ # # ]: 0 : for (auto pt : value.outer())
309 : : {
310 [ # # # # : 0 : stream << std::to_string(pt) << ",";
# # ]
311 : : }
312 [ # # ]: 0 : stream << "]";
313 [ # # ]: 0 : return stream.str();
314 : : }
315 : :
316 : : /*!
317 : : * @brief to_string overload for Line
318 : : *
319 : : * @param[in] value a line
320 : : *
321 : : * @returns string describing line
322 : : */
323 : : inline std::string to_string(ad::rss::unstructured::Line value)
324 : : {
325 : : std::stringstream stream;
326 : : stream << "[";
327 : : for (auto pt : value)
328 : : {
329 : : stream << "[" << pt.x() << "," << pt.y() << "],";
330 : : }
331 : : stream << "]";
332 : : return stream.str();
333 : : }
334 : : } // namespace std
|
