Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: MIT
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/map/lane/BorderOperation.hpp"
10 : : #include <algorithm>
11 : : #include "LaneOperationPrivate.hpp"
12 : : #include "ad/map/access/Logging.hpp"
13 : : #include "ad/map/point/Operation.hpp"
14 : :
15 : : namespace ad {
16 : : namespace map {
17 : : namespace lane {
18 : :
19 : 1 : physics::Distance calcLength(ENUBorderList const &borderList)
20 : : {
21 : 1 : physics::Distance total(0.);
22 [ + + ]: 4 : for (auto const &value : borderList)
23 : : {
24 [ + - + - ]: 3 : total = total + calcLength(value);
25 : : }
26 : 1 : return total;
27 : : }
28 : :
29 : 0 : physics::Distance calcLength(ECEFBorderList const &borderList)
30 : : {
31 : 0 : physics::Distance total(0.);
32 [ # # ]: 0 : for (auto const &value : borderList)
33 : : {
34 [ # # # # ]: 0 : total = total + calcLength(value);
35 : : }
36 : 0 : return total;
37 : : }
38 : :
39 : 0 : physics::Distance calcLength(GeoBorderList const &borderList)
40 : : {
41 : 0 : physics::Distance total(0.);
42 [ # # ]: 0 : for (auto const &value : borderList)
43 : : {
44 [ # # # # ]: 0 : total = total + calcLength(value);
45 : : }
46 : 0 : return total;
47 : : }
48 : :
49 : 6 : void removeDegeneratedEdgePoints(point::ENUEdge &edge, point::ENUEdge const *previousEdge = nullptr)
50 : : {
51 [ - + ]: 6 : if (edge.size() < 2u)
52 : : {
53 : 0 : return;
54 : : }
55 : :
56 [ + - ]: 6 : point::ENUPoint previousEdgeDir;
57 [ + + ]: 6 : if (previousEdge != nullptr)
58 : : {
59 [ - + ]: 2 : if (previousEdge->size() < 2u)
60 : : {
61 : 0 : return;
62 : : }
63 : : // decide if edge start point has to be adapted
64 [ + - + - : 2 : previousEdgeDir = previousEdge->at(previousEdge->size() - 1u) - previousEdge->at(previousEdge->size() - 2u);
+ - ]
65 [ + - + - ]: 2 : auto nextEdgeDir = edge[0] - previousEdge->at(previousEdge->size() - 1u);
66 : :
67 : : // if the two edges are near to each other or the direction is dramatically different,
68 : : // the start point is overwritten by the end of the previous edge
69 [ + - + - ]: 2 : if ((point::vectorLength(nextEdgeDir) > point::cEdgePointBorderDistance)
70 [ - + - - : 2 : || (point::vectorDotProduct(previousEdgeDir, nextEdgeDir) < 0.))
+ - ]
71 : : {
72 [ + - ]: 2 : edge[0] = previousEdge->at(previousEdge->size() - 1u);
73 : : }
74 : : else
75 : : {
76 : 0 : previousEdgeDir = nextEdgeDir;
77 : : }
78 : : }
79 : : else
80 : : {
81 [ + - ]: 4 : previousEdgeDir = edge[1] - edge[0];
82 : : }
83 : :
84 : 6 : std::size_t pointsToDrop = 0u;
85 [ + + ]: 14 : for (std::size_t i = 1u; i < edge.size(); ++i)
86 : : {
87 : : // i-pointsToDrop > 0
88 [ - + ]: 8 : if (pointsToDrop > 0u)
89 : : {
90 : 0 : edge[i - pointsToDrop] = edge[i];
91 : : }
92 [ + - ]: 8 : auto nextEdgeDir = edge[i - pointsToDrop] - edge[i - pointsToDrop - 1];
93 : 8 : auto dotProduct = point::vectorDotProduct(previousEdgeDir, nextEdgeDir);
94 [ - + ]: 8 : if (dotProduct < 0.)
95 : : {
96 : 0 : pointsToDrop++;
97 : : }
98 : : else
99 : : {
100 : 8 : previousEdgeDir = nextEdgeDir;
101 : : }
102 : : }
103 [ - + ]: 6 : if (pointsToDrop > 0u)
104 : : {
105 : 0 : std::size_t const newEdgeSize = std::max(std::size_t(2u), edge.size() - pointsToDrop);
106 [ # # ]: 0 : edge.resize(newEdgeSize);
107 : : }
108 : : }
109 : :
110 : : std::vector<std::size_t>
111 : 3 : calcSmallerEdgeIndexPairs(std::vector<physics::ParametricValue> const &biggerEdgeParametricPoints,
112 : : std::vector<physics::ParametricValue> const &smallerEdgeParametricPoints)
113 : : {
114 : 3 : std::vector<std::size_t> indexPairs;
115 [ + - ]: 3 : indexPairs.reserve(biggerEdgeParametricPoints.size());
116 [ + - ]: 3 : indexPairs.push_back(0u);
117 : 3 : std::size_t matchingSmallerIndex = 0u;
118 [ + + ]: 9 : for (std::size_t biggerIndex = 1u; biggerIndex < biggerEdgeParametricPoints.size(); biggerIndex++)
119 : : {
120 : 6 : physics::ParametricValue minParametricDistance(1.);
121 : 6 : bool matchingIndexFound = false;
122 : : // since both lists are sorted in ascending, it's ok to start search at last found index
123 : 15 : for (std::size_t smallerIndex = matchingSmallerIndex;
124 [ + - + + : 15 : (!matchingIndexFound) && (smallerIndex < smallerEdgeParametricPoints.size());
+ + ]
125 : : smallerIndex++)
126 : : {
127 : : auto const parametricDistance
128 [ + - ]: 9 : = std::fabs(smallerEdgeParametricPoints[smallerIndex] - biggerEdgeParametricPoints[biggerIndex]);
129 [ + - - + ]: 9 : if (parametricDistance > minParametricDistance)
130 : : {
131 : : // the best matching edge point is the one we have seen last
132 : 0 : matchingSmallerIndex = smallerIndex - 1u;
133 : 0 : matchingIndexFound = true;
134 [ # # ]: 0 : indexPairs.push_back(matchingSmallerIndex);
135 : : }
136 : : else
137 : : {
138 : 9 : minParametricDistance = parametricDistance;
139 : : }
140 : : }
141 [ + - ]: 6 : if (!matchingIndexFound)
142 : : {
143 : : // if not yet found, the last is the best matching one
144 : 6 : matchingSmallerIndex = smallerEdgeParametricPoints.size() - 1u;
145 [ + - ]: 6 : indexPairs.push_back(matchingSmallerIndex);
146 : : }
147 : : }
148 : 6 : return indexPairs;
149 : : }
150 : :
151 : 14 : point::ENUPoint interpolatePoint(point::ENUPoint const &pointBefore,
152 : : point::ENUPoint const &pointAfter,
153 : : physics::ParametricValue const pointBeforeParametricEdgeOffset,
154 : : physics::ParametricValue const pointInterpolatedParametricEdgeOffset,
155 : : physics::ParametricValue const pointAfterParametricEdgeOffset)
156 : : {
157 : : physics::ParametricValue const parametricOffset(
158 [ + - ]: 14 : (pointInterpolatedParametricEdgeOffset - pointBeforeParametricEdgeOffset)
159 [ + - + - ]: 28 : / (pointAfterParametricEdgeOffset - pointBeforeParametricEdgeOffset));
160 [ + - ]: 28 : return point::vectorInterpolate(pointBefore, pointAfter, parametricOffset);
161 : : }
162 : :
163 : 2 : void fillSmallerEdge(point::ENUEdge const &biggerEdge, point::ENUEdge &smallerEdge)
164 : : {
165 [ + - ]: 4 : auto const biggerEdgeParametricPoints = getParametricEdgePoints(biggerEdge);
166 [ + - ]: 4 : auto const smallerEdgeParametricPoints = getParametricEdgePoints(smallerEdge);
167 : :
168 [ + - ]: 4 : auto const smallerEdgeIndices = calcSmallerEdgeIndexPairs(biggerEdgeParametricPoints, smallerEdgeParametricPoints);
169 : :
170 : 4 : point::ENUEdge newSmallerEdge;
171 [ + - ]: 2 : newSmallerEdge.reserve(biggerEdge.size());
172 [ + - ]: 2 : newSmallerEdge.push_back(smallerEdge.front());
173 : 2 : std::size_t lastWrittenSmallerEdgeIndex = 0u;
174 [ + + ]: 4 : for (std::size_t i = 1u; i < smallerEdgeIndices.size() - 1u; ++i)
175 : : {
176 : : // i-1, i, i+1 indices are valid for smallerEdgeIndices and biggerEdgeParametricPoints
177 : :
178 : 2 : if ( // smaller point already processed, insert an interpolated
179 : 2 : (smallerEdgeIndices[i] == lastWrittenSmallerEdgeIndex)
180 : : // the point before the last point of the smaller edge is already written, so we have to interpolate till the end
181 [ + - + - : 2 : || (lastWrittenSmallerEdgeIndex == (smallerEdge.size() - 2u)))
+ - ]
182 : : {
183 : 2 : auto interpolatedPoint = interpolatePoint(smallerEdge[lastWrittenSmallerEdgeIndex],
184 : 2 : smallerEdge[lastWrittenSmallerEdgeIndex + 1u],
185 : 2 : biggerEdgeParametricPoints[i - 1],
186 : 2 : biggerEdgeParametricPoints[i],
187 [ + - ]: 4 : biggerEdgeParametricPoints[i + 1]);
188 [ + - ]: 2 : newSmallerEdge.push_back(interpolatedPoint);
189 : : }
190 : : else
191 : : {
192 : 0 : lastWrittenSmallerEdgeIndex = smallerEdgeIndices[i];
193 [ # # ]: 0 : newSmallerEdge.push_back(smallerEdge[lastWrittenSmallerEdgeIndex]);
194 : : }
195 : : }
196 [ + - ]: 2 : newSmallerEdge.push_back(smallerEdge.back());
197 : 2 : smallerEdge.swap(newSmallerEdge);
198 : 2 : }
199 : :
200 : 3 : void normalizeBorder(ENUBorder &border, ENUBorder const *previousBorder)
201 : : {
202 [ + - - + : 3 : if ((border.left.size() < 2u) || (border.right.size() < 2u))
- + ]
203 : : {
204 : 1 : return;
205 : : }
206 [ + - + + : 3 : if ((previousBorder != nullptr) && (previousBorder->left.size() >= 2u) && (previousBorder->right.size() >= 2u))
+ - + + ]
207 : : {
208 [ + - ]: 1 : removeDegeneratedEdgePoints(border.left, &previousBorder->left);
209 [ + - ]: 1 : removeDegeneratedEdgePoints(border.right, &previousBorder->right);
210 : : }
211 : : else
212 : : {
213 [ + - ]: 2 : removeDegeneratedEdgePoints(border.left);
214 [ + - ]: 2 : removeDegeneratedEdgePoints(border.right);
215 : : }
216 [ + + ]: 3 : if (border.left.size() == border.right.size())
217 : : {
218 : 1 : return;
219 : : }
220 : :
221 : 4 : std::vector<std::size_t> smallerEdgeIndices;
222 [ + - ]: 2 : if (border.left.size() > border.right.size())
223 : : {
224 [ + - ]: 2 : fillSmallerEdge(border.left, border.right);
225 : : }
226 : : else
227 : : {
228 [ # # ]: 0 : fillSmallerEdge(border.right, border.left);
229 : : }
230 : : }
231 : :
232 : 2 : IndexPairs getIndexPairs(point::ENUEdge const &leftEdge, point::ENUEdge const &rightEdge)
233 : : {
234 : 2 : std::size_t const maxSize = std::max(leftEdge.size(), rightEdge.size());
235 : 4 : std::vector<std::size_t> equalIndices;
236 [ + - ]: 2 : equalIndices.reserve(maxSize);
237 [ + + ]: 7 : for (size_t i = 0u; i < maxSize; ++i)
238 : : {
239 [ + - ]: 5 : equalIndices.push_back(i);
240 : : }
241 : :
242 : 2 : IndexPairs indexPairs;
243 [ + + ]: 2 : if (leftEdge.size() == rightEdge.size())
244 : : {
245 [ + - ]: 1 : indexPairs.leftEdgeIndices = equalIndices;
246 : 1 : indexPairs.rightEdgeIndices.swap(equalIndices);
247 : : }
248 [ + - ]: 1 : else if (leftEdge.size() > rightEdge.size())
249 : : {
250 : 1 : indexPairs.leftEdgeIndices.swap(equalIndices);
251 : : indexPairs.rightEdgeIndices
252 [ + - + - : 1 : = calcSmallerEdgeIndexPairs(getParametricEdgePoints(leftEdge), getParametricEdgePoints(rightEdge));
+ - ]
253 : : }
254 : : else
255 : : {
256 : : indexPairs.leftEdgeIndices
257 [ # # # # : 0 : = calcSmallerEdgeIndexPairs(getParametricEdgePoints(rightEdge), getParametricEdgePoints(leftEdge));
# # ]
258 : 0 : indexPairs.rightEdgeIndices.swap(equalIndices);
259 : : }
260 : 4 : return indexPairs;
261 : : }
262 : :
263 : 1 : point::ENUEdge getLateralAlignmentEdge(ENUBorder const &border, physics::ParametricValue const lateralAlignment)
264 : : {
265 : 1 : return point::getLateralAlignmentEdge(border.left, border.right, lateralAlignment);
266 : : }
267 : :
268 : 0 : inline bool isPointWithinBorderPoints(point::ENUPoint const &ptLeft,
269 : : point::ENUPoint const &ptRight,
270 : : point::ENUPoint const &enuPoint)
271 : : {
272 : : // check if the point is actually located in between the border points
273 [ # # ]: 0 : auto const towardsLeft = ptLeft - enuPoint;
274 [ # # ]: 0 : auto const towardsRight = ptRight - enuPoint;
275 : 0 : auto const dotProduct = point::vectorDotProduct(towardsLeft, towardsRight);
276 [ # # # # : 0 : auto const withinBorders = (dotProduct < 0.) || (point::vectorLength(towardsLeft) < physics::Distance(0.01))
# # ]
277 [ # # # # : 0 : || (point::vectorLength(towardsRight) < physics::Distance(0.01));
# # # # ]
278 : 0 : return withinBorders;
279 : : }
280 : :
281 : 0 : inline point::ENUHeading createHeadingFromBorderPoints(point::ENUPoint const &ptLeft, point::ENUPoint const &ptRight)
282 : : {
283 [ # # ]: 0 : auto const headingLeftToRight = point::createENUHeading(ptLeft, ptRight);
284 [ # # ]: 0 : auto const resultHeading = point::createENUHeading(static_cast<double>(headingLeftToRight) + M_PI_2);
285 : 0 : return resultHeading;
286 : : }
287 : :
288 : 0 : point::ENUHeading getENUHeading(ENUBorderList const &borderList, point::ENUPoint const &enuPoint)
289 : : {
290 : 0 : point::ENUHeading resultHeading(2. * M_PI);
291 : :
292 : 0 : auto firstBeforeBorderDetected = borderList.end();
293 : 0 : auto lastAfterBorderDetected = borderList.end();
294 [ # # ]: 0 : for (auto borderIter = borderList.begin(); borderIter != borderList.end(); borderIter++)
295 : : {
296 [ # # ]: 0 : auto const edgeLeftLength = calcLength(borderIter->left);
297 [ # # ]: 0 : auto const edgeRightLength = calcLength(borderIter->right);
298 : :
299 : : // first of all we have to handle degenerated borders
300 [ # # # # : 0 : if ((edgeLeftLength == physics::Distance(0.)) || (edgeRightLength == physics::Distance(0.)))
# # # # #
# ]
301 : : {
302 [ # # # # : 0 : if ((borderIter->left.size() == 0u) || (borderIter->right.size() == 0u))
# # ]
303 : : {
304 [ # # # # ]: 0 : access::getLogger()->error(
305 : : "ad::map::lane::getENUHeading: invalid enu border of size border-left:{} border-rigth:{} query-point:{}!",
306 : 0 : borderIter->left,
307 : 0 : borderIter->right,
308 : : enuPoint);
309 : 0 : return resultHeading;
310 : : }
311 : :
312 : : // the question here is: are we actually before or after the border
313 [ # # ]: 0 : auto const headingLeftToRight = point::createENUHeading(borderIter->left[0], borderIter->right[0]);
314 [ # # ]: 0 : auto const headingLeftToPoint = point::createENUHeading(borderIter->left[0], enuPoint);
315 [ # # # # ]: 0 : auto const headingDiff = point::createENUHeading(static_cast<double>(headingLeftToPoint - headingLeftToRight));
316 [ # # # # ]: 0 : if (headingDiff < point::ENUHeading(0.))
317 : : {
318 [ # # ]: 0 : if (firstBeforeBorderDetected == borderList.end())
319 : : {
320 : 0 : firstBeforeBorderDetected = borderIter;
321 : : }
322 : : }
323 : : else
324 : : {
325 : 0 : lastAfterBorderDetected = borderIter;
326 : : }
327 : : }
328 : : else
329 : : {
330 [ # # ]: 0 : auto parametricOffsetLeft = point::findNearestPointOnEdge(borderIter->left, edgeLeftLength, enuPoint);
331 [ # # ]: 0 : auto parametricOffsetRight = point::findNearestPointOnEdge(borderIter->right, edgeRightLength, enuPoint);
332 : :
333 : : // catch errors
334 [ # # # # : 0 : if ((!parametricOffsetLeft.isValid()) || (!parametricOffsetRight.isValid()))
# # ]
335 : : {
336 [ # # # # ]: 0 : access::getLogger()->error(
337 : : "ad::map::lane::getENUHeading: invalid input data border-left:{} border-rigth:{} query-point:{}",
338 : 0 : borderIter->left,
339 : 0 : borderIter->right,
340 : : enuPoint);
341 : 0 : return resultHeading;
342 : : }
343 : :
344 [ # # ]: 0 : if ((parametricOffsetLeft == physics::ParametricValue(0.))
345 [ # # # # : 0 : || (parametricOffsetRight == physics::ParametricValue(0.)))
# # # # ]
346 : : {
347 [ # # ]: 0 : if (firstBeforeBorderDetected == borderList.end())
348 : : {
349 : 0 : firstBeforeBorderDetected = borderIter;
350 : : }
351 : : }
352 [ # # ]: 0 : else if ((parametricOffsetLeft == physics::ParametricValue(1.))
353 [ # # # # : 0 : || (parametricOffsetRight == physics::ParametricValue(1.)))
# # # # ]
354 : : {
355 : 0 : lastAfterBorderDetected = borderIter;
356 : : }
357 : : else
358 : : {
359 : : // looks as we have found a potential segment
360 [ # # ]: 0 : auto const ptLeft = point::getParametricPoint(borderIter->left, parametricOffsetLeft);
361 [ # # ]: 0 : auto const ptRight = point::getParametricPoint(borderIter->right, parametricOffsetRight);
362 [ # # ]: 0 : bool const withinBorder = isPointWithinBorderPoints(ptLeft, ptRight, enuPoint);
363 : :
364 [ # # ]: 0 : if (withinBorder)
365 : : {
366 [ # # ]: 0 : return createHeadingFromBorderPoints(ptLeft, ptRight);
367 : : }
368 : : }
369 : : }
370 : : }
371 : :
372 : : // if we didn't find any actual match in between so far, use either the first before or the last after to create the
373 : : // heading
374 [ # # ]: 0 : if (firstBeforeBorderDetected != borderList.end())
375 : : {
376 : 0 : auto const ptLeft = firstBeforeBorderDetected->left.front();
377 : 0 : auto const ptRight = firstBeforeBorderDetected->right.front();
378 [ # # ]: 0 : bool const withinBorder = isPointWithinBorderPoints(ptLeft, ptRight, enuPoint);
379 [ # # ]: 0 : if (withinBorder)
380 : : {
381 [ # # ]: 0 : return createHeadingFromBorderPoints(ptLeft, ptRight);
382 : : }
383 : : }
384 [ # # ]: 0 : if (lastAfterBorderDetected != borderList.end())
385 : : {
386 : 0 : auto const ptLeft = lastAfterBorderDetected->left.back();
387 : 0 : auto const ptRight = lastAfterBorderDetected->right.back();
388 [ # # ]: 0 : bool const withinBorder = isPointWithinBorderPoints(ptLeft, ptRight, enuPoint);
389 [ # # ]: 0 : if (withinBorder)
390 : : {
391 [ # # ]: 0 : return createHeadingFromBorderPoints(ptLeft, ptRight);
392 : : }
393 : : }
394 : :
395 : 0 : return resultHeading;
396 : : }
397 : :
398 : 1 : physics::Distance getDistanceEnuPointToLateralAlignmentEdge(point::ENUPoint const &enuPoint,
399 : : point::ENUEdge const &lateralAlignmentEdge)
400 : : {
401 [ + - ]: 1 : auto const edgeLength = calcLength(lateralAlignmentEdge);
402 [ + - ]: 1 : auto const parametricOffset = point::findNearestPointOnEdge(lateralAlignmentEdge, edgeLength, enuPoint);
403 [ + - ]: 1 : auto const projectedPoint = point::getParametricPoint(lateralAlignmentEdge, edgeLength, parametricOffset);
404 [ + - ]: 1 : auto const distance = point::distance(projectedPoint, enuPoint);
405 : 1 : return distance;
406 : : }
407 : :
408 : 12 : bool areEdgesContinuous(point::ENUEdge const &first, point::ENUEdge const &second)
409 : : {
410 [ + - - + : 12 : if ((first.size() < 2u) || (second.size() < 2u))
- + ]
411 : : {
412 : 0 : return true;
413 : : }
414 : :
415 [ + - ]: 12 : auto const firstToSecondDistance = point::distance(second.front(), first.back());
416 [ + - - + ]: 12 : if (firstToSecondDistance < point::cEdgePointBorderDistance)
417 : : {
418 : 0 : return true;
419 : : }
420 : 12 : return false;
421 : : }
422 : :
423 : 8 : void performMakeTransitionToSecondEdgeContinuous(point::ENUEdge const &first,
424 : : point::ENUEdge &second,
425 : : point::ENUEdge *secondOtherLane = nullptr)
426 : : {
427 [ + - - + ]: 8 : if (areEdgesContinuous(first, second))
428 : : {
429 : 0 : return;
430 : : }
431 [ + - ]: 8 : auto const firstToSecondVec = second.front() - first.back();
432 [ + - ]: 8 : auto const firstToSecondDistance = point::vectorLength(firstToSecondVec);
433 [ + - ]: 8 : auto const firstToSecondDir = point::vectorNorm(firstToSecondVec);
434 [ + - + - ]: 8 : auto const firstEndDir = getEdgeEndDirectionalVector(first);
435 [ + - + - ]: 8 : auto const secondStartDir = getEdgeStartDirectionalVector(second);
436 [ + - + - ]: 8 : auto const averageEdgeDir = 0.5 * (firstEndDir + secondStartDir);
437 : : double const sinAngle
438 [ + - + - ]: 8 : = static_cast<double>(point::vectorLength(point::vectorCrossProduct(averageEdgeDir, firstToSecondDir)));
439 : : // the distance of our interpolation changes depend on the offset of the two edges ends and the lateral offset
440 : : // (given by the sine of the angle between the general edge direction and the offset direction)
441 : : // e.g. if the offset is 3m and orthogonal we get a 6m interpolating distance
442 [ + - ]: 8 : auto const interpolatingDistance = sinAngle * 2.0 * firstToSecondDistance;
443 : :
444 [ + - ]: 8 : auto const secondLength = calcLength(second);
445 : :
446 [ + - ]: 8 : physics::ParametricValue const parametricInterpolatingDistance(std::min(.95, interpolatingDistance / secondLength));
447 : :
448 [ + - ]: 16 : auto const secondParametricEdgePoints = getParametricEdgePoints(second);
449 : :
450 : 8 : std::size_t const newSecondSize = second.size() + 1u;
451 : 16 : point::ENUEdge newSecond;
452 [ + - ]: 8 : newSecond.reserve(newSecondSize);
453 : 16 : point::ENUEdge newOtherSecond;
454 [ + + + - : 8 : if ((secondOtherLane != nullptr) && (secondOtherLane->size() == second.size()))
+ + ]
455 : : {
456 [ + - ]: 4 : newOtherSecond.resize(newSecondSize);
457 : : }
458 [ + + ]: 18 : for (std::size_t i = 1u; i < secondParametricEdgePoints.size(); ++i)
459 : : {
460 [ + - + - ]: 10 : if (secondParametricEdgePoints[i] >= parametricInterpolatingDistance)
461 : : {
462 [ + + ]: 10 : if (newSecond.empty())
463 : : {
464 : : // first point found
465 : :
466 : : // take the last point of the first edge
467 [ + - ]: 8 : newSecond.push_back(first.back());
468 : :
469 : : // calculate the interpolated point
470 : 8 : point::ENUPoint interpolatedPoint = interpolatePoint(second[i - 1],
471 : 8 : second[i],
472 : 8 : secondParametricEdgePoints[i - 1],
473 : : parametricInterpolatingDistance,
474 [ + - ]: 16 : secondParametricEdgePoints[i]);
475 : :
476 : : //@todo: one potential issue is not considered in here yet:
477 : : // if second curvature is extreme, the newly created linear edge segment by the above two points could
478 : : // potentially intersect with the old second border and by that enlarging a driving area beyond the
479 : : // allowed borders. To prevent from this one would have to implement 'point::vectorIntersects':
480 : : //
481 : : // auto const newEdgeSegment = interpolatedPoint-newSecond[0];
482 : : // for (std::size_t j=1u; j<i; j++ )
483 : : // {
484 : : // auto const checkSegment = second[j] - second[j-1u];
485 : : // if ( point::vectorIntersects(checkSegment, newEdgeSegment) )
486 : : // {
487 : : // i = j;
488 : : // std::size_t const newSecondSize = secondParametricEdgePoints.size()-i + 1u;
489 : : // newSecond.reserve(newSecondSize);
490 : : // interpolatedPoint = secondParametricEdgePoints[i-1];
491 : : // break;
492 : : // }
493 : : // }
494 : :
495 : : // before adding the interpolated point we have to add also some interpolated points on the vector
496 : : // newSecond[0] -> interpolatedPoint if there have been others skipped
497 [ - + ]: 8 : for (std::size_t j = 1u; j < i; ++j)
498 : : {
499 : 0 : auto innerInterpolatedPoint = interpolatePoint(newSecond[0],
500 : : interpolatedPoint,
501 : : physics::ParametricValue(0.),
502 : 0 : secondParametricEdgePoints[j],
503 [ # # ]: 0 : parametricInterpolatingDistance);
504 [ # # ]: 0 : newSecond.push_back(innerInterpolatedPoint);
505 : : }
506 : :
507 : : // then we add the interpolated point
508 [ + - ]: 8 : newSecond.push_back(interpolatedPoint);
509 : :
510 : : // fill the newOtherSecond lane if available
511 [ + + ]: 8 : if (newOtherSecond.size() > 0u)
512 : : {
513 : 4 : std::size_t j = 0u;
514 [ + + + - : 8 : for (; (j < i) && (j < newOtherSecond.size()) && (j < secondOtherLane->size()); ++j)
+ - + + ]
515 : : {
516 [ + - ]: 4 : newOtherSecond[j] = secondOtherLane->at(j);
517 : : }
518 [ + - + - : 4 : if ((j < newOtherSecond.size()) && (j < secondOtherLane->size()))
+ - ]
519 : : {
520 : : // same parametric range than the interpolated point
521 : 4 : newOtherSecond[j] = interpolatePoint(secondOtherLane->at(j - 1u),
522 [ + - + - ]: 4 : secondOtherLane->at(j),
523 : 4 : secondParametricEdgePoints[i - 1],
524 : : parametricInterpolatingDistance,
525 [ + - ]: 8 : secondParametricEdgePoints[i]);
526 : : }
527 [ + + + - : 10 : for (; ((j + 1u) < newOtherSecond.size()) && (j < secondOtherLane->size()); j++)
+ + ]
528 : : {
529 [ + - ]: 6 : newOtherSecond[j + 1u] = secondOtherLane->at(j);
530 : : }
531 : : }
532 : : }
533 : : // add this one
534 [ + - ]: 10 : newSecond.push_back(second[i]);
535 : : }
536 : : }
537 : 8 : second.swap(newSecond);
538 [ + + ]: 8 : if (secondOtherLane != nullptr)
539 : : {
540 : 4 : secondOtherLane->swap(newOtherSecond);
541 : : }
542 : : }
543 : :
544 : 2 : void makeTransitionToSecondEdgeContinuous(point::ENUEdge const &first, point::ENUEdge &second)
545 : : {
546 : 2 : performMakeTransitionToSecondEdgeContinuous(first, second);
547 : 2 : }
548 : :
549 : 2 : void makeTransitionFromFirstEdgeContinuous(point::ENUEdge &first, point::ENUEdge const &second)
550 : : {
551 [ + - - + ]: 2 : if (areEdgesContinuous(first, second))
552 : : {
553 : 0 : return;
554 : : }
555 : : // if actually something has to be calculated, we reverse the edge and let the other function do the job
556 [ + - ]: 4 : point::ENUEdge invertedFirstEdge = first;
557 [ + - ]: 2 : std::reverse(invertedFirstEdge.begin(), invertedFirstEdge.end());
558 [ + - ]: 4 : point::ENUEdge invertedSecondEdge = second;
559 [ + - ]: 2 : std::reverse(invertedSecondEdge.begin(), invertedSecondEdge.end());
560 [ + - ]: 2 : performMakeTransitionToSecondEdgeContinuous(invertedSecondEdge, invertedFirstEdge);
561 [ + - ]: 2 : first = invertedFirstEdge;
562 [ + - ]: 2 : std::reverse(first.begin(), first.end());
563 : : }
564 : :
565 : 2 : void makeTransitionToSecondBorderContinuous(ENUBorder const &first, ENUBorder &second)
566 : : {
567 : 2 : performMakeTransitionToSecondEdgeContinuous(first.left, second.left, &second.right);
568 : 2 : performMakeTransitionToSecondEdgeContinuous(first.right, second.right, &second.left);
569 : 2 : }
570 : :
571 : 2 : void makeTransitionFromFirstBorderContinuous(ENUBorder &first, ENUBorder const &second)
572 : : {
573 [ + - - + : 2 : if (areEdgesContinuous(first.left, second.left) && areEdgesContinuous(first.right, second.right))
- - - - -
+ ]
574 : : {
575 : 0 : return;
576 : : }
577 : : // if actually something has to be calculated, we reverse the borders and let the other function do the job
578 : 4 : ENUBorder invertedFirstBorder;
579 [ + - ]: 2 : invertedFirstBorder.left = first.left;
580 [ + - ]: 2 : invertedFirstBorder.right = first.right;
581 [ + - ]: 2 : std::reverse(invertedFirstBorder.left.begin(), invertedFirstBorder.left.end());
582 [ + - ]: 2 : std::reverse(invertedFirstBorder.right.begin(), invertedFirstBorder.right.end());
583 : 4 : ENUBorder invertedSecondBorder;
584 [ + - ]: 2 : invertedSecondBorder.left = second.left;
585 [ + - ]: 2 : invertedSecondBorder.right = second.right;
586 [ + - ]: 2 : std::reverse(invertedSecondBorder.left.begin(), invertedSecondBorder.left.end());
587 [ + - ]: 2 : std::reverse(invertedSecondBorder.right.begin(), invertedSecondBorder.right.end());
588 [ + - ]: 2 : makeTransitionToSecondBorderContinuous(invertedSecondBorder, invertedFirstBorder);
589 [ + - ]: 2 : first = invertedFirstBorder;
590 [ + - ]: 2 : std::reverse(first.left.begin(), first.left.end());
591 [ + - ]: 2 : std::reverse(first.right.begin(), first.right.end());
592 : : }
593 : :
594 : : } // namespace lane
595 : : } // namespace map
596 : : } // namespace ad
|
