Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2020-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : : /**
9 : : * @file
10 : : */
11 : :
12 : : #include "ad/rss/unstructured/Geometry.hpp"
13 : : #include <ad/physics/Operation.hpp>
14 : : #include <algorithm>
15 : : #include <cmath>
16 : : #include <limits>
17 : :
18 : : /*!
19 : : * @brief namespace ad
20 : : */
21 : : namespace ad {
22 : : /*!
23 : : * @brief namespace rss
24 : : */
25 : : namespace rss {
26 : : /*!
27 : : * @brief namespace situation
28 : : */
29 : : namespace unstructured {
30 : :
31 : 7461 : ad::rss::unstructured::Point rotateAroundPoint(ad::rss::unstructured::Point const &origin,
32 : : ad::rss::unstructured::Point const &relativePoint,
33 : : ad::physics::Angle const &angle)
34 : : {
35 : 7461 : ad::rss::unstructured::Point resultPoint = relativePoint;
36 : :
37 : : boost::geometry::strategy::transform::rotate_transformer<boost::geometry::radian, double, 2, 2> rotate(
38 [ + - + - ]: 7461 : static_cast<double>(-angle));
39 [ + - + - : 7461 : boost::geometry::strategy::transform::translate_transformer<double, 2, 2> translate(origin.x(), origin.y());
+ - ]
40 [ + - ]: 7461 : boost::geometry::transform(resultPoint, resultPoint, rotate);
41 [ + - ]: 7461 : boost::geometry::transform(resultPoint, resultPoint, translate);
42 : 7461 : return resultPoint;
43 : : }
44 : :
45 : 5 : ad::rss::unstructured::Point getPointOnCircle(ad::rss::unstructured::Point const &origin,
46 : : ad::physics::Distance const &radius,
47 : : ad::physics::Angle const &angle)
48 : : {
49 [ + - + - : 10 : return origin + toPoint(std::cos(angle) * radius, std::sin(angle) * radius);
+ - + - +
- ]
50 : : }
51 : :
52 : 4421 : ad::rss::unstructured::Point getCircleOrigin(ad::rss::unstructured::Point const &point,
53 : : ad::physics::Distance const &radius,
54 : : ad::physics::Angle const &angle)
55 : : {
56 [ + - + - : 8842 : return point - toPoint(std::cos(angle) * radius, std::sin(angle) * radius);
+ - + - +
- ]
57 : : }
58 : :
59 : 0 : ad::physics::Distance getDistance(ad::rss::unstructured::Point const &point1,
60 : : ad::rss::unstructured::Point const &point2)
61 : : {
62 [ # # ]: 0 : auto const directionalVector = point1 - point2;
63 [ # # ]: 0 : return ad::physics::Distance(sqrt(boost::geometry::dot_product(directionalVector, directionalVector)));
64 : : }
65 : :
66 : 91 : void toPolygon(ad::rss::world::UnstructuredTrajectorySet const &trajectorySet, ad::rss::unstructured::Polygon &polygon)
67 : : {
68 [ + + ]: 3657 : for (auto const &distance : trajectorySet)
69 : : {
70 [ + - + - ]: 3566 : ::boost::geometry::append(polygon, toPoint(distance));
71 : : }
72 : 91 : }
73 : :
74 : 41 : void toTrajectorySet(unstructured::Polygon const &polygon, world::UnstructuredTrajectorySet &trajectorySet)
75 : : {
76 [ + + ]: 956 : for (auto const &point : polygon.outer())
77 : : {
78 [ + - + - ]: 915 : trajectorySet.push_back(toDistance(point));
79 : : }
80 : 41 : }
81 : :
82 : 45 : bool collides(ad::rss::world::UnstructuredTrajectorySet const &trajectorySet1,
83 : : ad::rss::world::UnstructuredTrajectorySet const &trajectorySet2)
84 : : {
85 [ + - ]: 90 : ad::rss::unstructured::Polygon polygon1;
86 [ + - ]: 45 : ad::rss::unstructured::Polygon polygon2;
87 [ + - ]: 45 : toPolygon(trajectorySet1, polygon1);
88 [ + - ]: 45 : toPolygon(trajectorySet2, polygon2);
89 [ + - ]: 90 : return !boost::geometry::disjoint(polygon1, polygon2);
90 : : }
91 : :
92 : 80 : bool isInsideHeadingRange(ad::physics::Angle const &angle, ad::rss::state::HeadingRange const &range)
93 : : {
94 [ + + ]: 80 : if (range.end >= range.begin)
95 : : {
96 [ + + + + ]: 70 : return (angle >= range.begin) && (angle <= range.end);
97 : : }
98 : : else
99 : : {
100 [ + + + + ]: 10 : return (angle >= range.begin) || (angle <= range.end);
101 : : }
102 : : }
103 : :
104 : 18 : bool getHeadingOverlap(ad::rss::state::HeadingRange const &a,
105 : : ad::rss::state::HeadingRange const &b,
106 : : std::vector<ad::rss::state::HeadingRange> &overlapRanges)
107 : : {
108 : 18 : bool aStartsInB = isInsideHeadingRange(a.begin, b);
109 : 18 : bool aEndsInB = isInsideHeadingRange(a.end, b);
110 : 18 : bool bStartsInA = isInsideHeadingRange(b.begin, a);
111 : 18 : bool bEndsInA = isInsideHeadingRange(b.end, a);
112 : :
113 [ + + + + : 18 : auto overlaps = aStartsInB || aEndsInB || bStartsInA || bEndsInA;
+ + - + ]
114 : :
115 [ + + ]: 18 : if (overlaps)
116 : : {
117 [ + - ]: 12 : ad::rss::state::HeadingRange range;
118 [ + + + + : 12 : if (aStartsInB && aEndsInB && bStartsInA && bEndsInA)
+ + + + ]
119 : : {
120 [ + - + + : 3 : if (!((a.begin == b.begin) && (a.end == b.end)))
+ - - + +
+ ]
121 : : {
122 [ + - ]: 2 : ad::rss::state::HeadingRange secondRange;
123 : 2 : secondRange.begin = a.begin;
124 : 2 : secondRange.end = b.end;
125 [ + - ]: 2 : overlapRanges.push_back(secondRange);
126 : : }
127 : 3 : range.begin = b.begin;
128 : 3 : range.end = a.end;
129 : : }
130 [ + + + + ]: 9 : else if (aStartsInB && aEndsInB)
131 : : {
132 : 3 : range.begin = a.begin;
133 : 3 : range.end = a.end;
134 : : }
135 [ + + + + ]: 6 : else if (bStartsInA && bEndsInA)
136 : : {
137 : 3 : range.begin = b.begin;
138 : 3 : range.end = b.end;
139 : : }
140 [ + + + - ]: 3 : else if (aStartsInB && !aEndsInB)
141 : : {
142 : 2 : range.begin = a.begin;
143 : 2 : range.end = b.end;
144 : : }
145 [ + - + - ]: 1 : else if (bStartsInA && !bEndsInA)
146 : : {
147 : 1 : range.begin = b.begin;
148 : 1 : range.end = a.end;
149 : : }
150 [ + - ]: 12 : overlapRanges.push_back(range);
151 : 12 : return true;
152 : : }
153 : 6 : return false;
154 : : }
155 : :
156 : 11 : bool getHeadingOverlap(ad::rss::state::HeadingRange const &headingRange,
157 : : std::vector<ad::rss::state::HeadingRange> &overlapRanges)
158 : : {
159 : 11 : std::vector<ad::rss::state::HeadingRange> newOverlapRanges;
160 [ - + ]: 11 : if (overlapRanges.empty())
161 : : {
162 [ # # ]: 0 : newOverlapRanges.push_back(headingRange);
163 : : }
164 : : else
165 : : {
166 [ + + ]: 27 : for (auto const &range : overlapRanges)
167 : : {
168 [ + - ]: 16 : getHeadingOverlap(headingRange, range, newOverlapRanges);
169 : : }
170 : : }
171 [ + - ]: 11 : overlapRanges = newOverlapRanges;
172 : 22 : return !overlapRanges.empty();
173 : : }
174 : :
175 : 340 : bool combinePolygon(Polygon const &a, Polygon const &b, Polygon &result)
176 : : {
177 [ - + - - : 340 : if (a.outer().empty() && !b.outer().empty())
- + ]
178 : : {
179 : 0 : result = b;
180 : : }
181 [ + - - + : 340 : else if (!a.outer().empty() && b.outer().empty())
- + ]
182 : : {
183 : 0 : result = a;
184 : : }
185 : : else
186 : : {
187 : 340 : std::vector<Polygon> unionPolygons;
188 [ + - ]: 340 : boost::geometry::union_(a.outer(), b.outer(), unionPolygons);
189 [ - + ]: 340 : if (unionPolygons.size() != 1)
190 : : {
191 : 0 : auto fixed = false;
192 [ # # ]: 0 : if (unionPolygons.size() == 0)
193 : : {
194 : : // if union reports zero polygons, it might happen that one polygon is covered by the other
195 : 0 : auto aCoveredByB = true;
196 [ # # # # : 0 : for (auto it = a.outer().cbegin(); (it != a.outer().cend()) && aCoveredByB; ++it)
# # ]
197 : : {
198 [ # # ]: 0 : aCoveredByB &= boost::geometry::covered_by(*it, b.outer());
199 : : }
200 [ # # ]: 0 : if (aCoveredByB)
201 : : {
202 : 0 : fixed = true;
203 [ # # ]: 0 : result = b;
204 : : }
205 [ # # ]: 0 : if (!fixed)
206 : : {
207 : 0 : auto bCoveredByA = true;
208 [ # # # # : 0 : for (auto it = b.outer().cbegin(); (it != b.outer().cend()) && bCoveredByA; ++it)
# # ]
209 : : {
210 [ # # ]: 0 : bCoveredByA &= boost::geometry::covered_by(*it, a.outer());
211 : : }
212 [ # # ]: 0 : if (bCoveredByA)
213 : : {
214 : 0 : fixed = true;
215 [ # # ]: 0 : result = a;
216 : : }
217 : : }
218 : : }
219 [ # # ]: 0 : if (!fixed)
220 : : {
221 [ # # ]: 0 : spdlog::warn("Could not calculate combined polygon. Expected 1 polygon after union, found {}. A:\n{}\nB:\n{}",
222 : 0 : unionPolygons.size(),
223 [ # # ]: 0 : std::to_string(a),
224 [ # # ]: 0 : std::to_string(b));
225 : 0 : return false;
226 : : }
227 : : }
228 : : else
229 : : {
230 : 340 : result = std::move(unionPolygons[0]);
231 : : }
232 : : }
233 : 340 : return true;
234 : : }
235 : :
236 : : } // namespace unstructured
237 : : } // namespace rss
238 : : } // namespace ad
|
