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/route/LaneIntervalOperation.hpp"
10 : :
11 : : #include <algorithm>
12 : : #include "ad/map/access/Operation.hpp"
13 : : #include "ad/map/lane/LaneOperation.hpp"
14 : : #include "ad/map/point/Operation.hpp"
15 : :
16 : : namespace ad {
17 : : namespace map {
18 : : namespace route {
19 : :
20 : 2 : point::ParaPoint getIntervalStart(FullRoute const &route, lane::LaneId const &laneId)
21 : : {
22 [ + - ]: 2 : point::ParaPoint result;
23 [ + + ]: 2 : for (auto const &roadSegment : route.roadSegments)
24 : : {
25 : : auto it = find_if(roadSegment.drivableLaneSegments.begin(),
26 : : roadSegment.drivableLaneSegments.end(),
27 [ + - ]: 2 : [&laneId](const LaneSegment &l) { return l.laneInterval.laneId == laneId; });
28 : :
29 [ + - ]: 1 : if (it != roadSegment.drivableLaneSegments.end())
30 : : {
31 : 1 : result.laneId = laneId;
32 : 1 : result.parametricOffset = it->laneInterval.start;
33 : 1 : return result;
34 : : }
35 : : }
36 : :
37 [ + - ]: 1 : throw std::invalid_argument("ad::map::route::getIntervalStart: laneId not found in route");
38 : : }
39 : :
40 : : physics::ParametricValue
41 : 4 : getSignedDistance(LaneInterval const &laneInterval, point::ParaPoint const &first, point::ParaPoint const &second)
42 : : {
43 [ + + + + : 4 : if ((first.laneId != second.laneId) || (first.laneId != laneInterval.laneId))
+ + ]
44 : : {
45 [ + - ]: 2 : throw std::invalid_argument("ad::map::route::getSignedDistance: lane id's not matching");
46 : : }
47 : :
48 [ + + ]: 2 : if (isRouteDirectionPositive(laneInterval))
49 : : {
50 : 1 : return second.parametricOffset - first.parametricOffset;
51 : : }
52 : : else
53 : : {
54 : 1 : return first.parametricOffset - second.parametricOffset;
55 : : }
56 : : }
57 : :
58 : : physics::ParametricValue
59 : 3 : getUnsignedDistance(LaneInterval const &laneInterval, point::ParaPoint const &first, point::ParaPoint const &second)
60 : : {
61 [ + + + + : 3 : if ((first.laneId != second.laneId) || (first.laneId != laneInterval.laneId))
+ + ]
62 : : {
63 [ + - ]: 2 : throw std::invalid_argument("ad::map::route::getSignedDistance: lane id's not matching");
64 : : }
65 : 1 : return std::fabs(first.parametricOffset - second.parametricOffset);
66 : : }
67 : :
68 : 13091 : bool isRouteDirectionPositive(LaneInterval const &laneInterval)
69 : : {
70 [ + + ]: 13091 : if (laneInterval.start == laneInterval.end)
71 : : {
72 : 112 : return lane::isLaneDirectionPositive(laneInterval.laneId) ^ laneInterval.wrongWay;
73 : : }
74 : : else
75 : : {
76 : 12979 : return (laneInterval.start < laneInterval.end);
77 : : }
78 : : }
79 : :
80 : 174 : bool isRouteDirectionAlignedWithDrivingDirection(LaneInterval const &laneInterval)
81 : : {
82 [ + + ]: 174 : if (isRouteDirectionPositive(laneInterval))
83 : : {
84 : 148 : return lane::isLaneDirectionPositive(laneInterval.laneId);
85 : : }
86 : : else
87 : : {
88 : 26 : return lane::isLaneDirectionNegative(laneInterval.laneId);
89 : : }
90 : : }
91 : :
92 : 4 : physics::ParametricValue getProjectedParametricOffsetOnNeighborLane(LaneInterval const ¤tInterval,
93 : : LaneInterval const &neighborInterval,
94 : : physics::ParametricValue const ¶metricOffset)
95 : : {
96 [ + + - + ]: 4 : if (!lane::isSameOrDirectNeighbor(currentInterval.laneId, neighborInterval.laneId))
97 : : {
98 [ # # ]: 0 : throw std::invalid_argument("ad::map::route::getProjectedParametricOffsetOnNeighborLane: lanes are not neighbors");
99 : : }
100 : :
101 [ + - + + ]: 3 : if (currentInterval.laneId == neighborInterval.laneId)
102 : : {
103 : 1 : return parametricOffset;
104 : : }
105 : :
106 : : // real neighbors
107 [ + - + - ]: 4 : auto currentLane = lane::getLane(currentInterval.laneId);
108 [ + - + - ]: 4 : auto neighborLane = lane::getLane(neighborInterval.laneId);
109 [ + - ]: 4 : auto leftNeighbors = lane::getContactLanes(currentLane, lane::ContactLocation::LEFT);
110 [ + - ]: 2 : auto rightNeighbors = lane::getContactLanes(currentLane, lane::ContactLocation::RIGHT);
111 : :
112 [ + - ]: 2 : point::ECEFPoint leftECEFPoint;
113 [ + - ]: 2 : point::ECEFPoint rightECEFPoint;
114 : 2 : physics::ParametricValue offset;
115 [ + - ]: 2 : lane::projectParametricPointToEdges(currentLane, parametricOffset, leftECEFPoint, rightECEFPoint);
116 : :
117 [ + - + - : 2 : if ((leftNeighbors.size() > 0) && (leftNeighbors[0].toLane == neighborInterval.laneId))
+ + + + ]
118 : : {
119 [ + - ]: 1 : offset = ((point::findNearestPointOnEdge(neighborLane.edgeRight, leftECEFPoint)
120 [ + - ]: 1 : + point::findNearestPointOnEdge(neighborLane.edgeLeft, leftECEFPoint))
121 [ + - + - ]: 2 : / 2.);
122 : : }
123 [ + - + - : 1 : else if ((rightNeighbors.size() > 0) && (rightNeighbors[0].toLane == neighborInterval.laneId))
+ - + - ]
124 : : {
125 [ + - ]: 1 : offset = ((point::findNearestPointOnEdge(neighborLane.edgeRight, rightECEFPoint)
126 [ + - ]: 1 : + point::findNearestPointOnEdge(neighborLane.edgeLeft, rightECEFPoint))
127 [ + - + - ]: 2 : / 2.);
128 : : }
129 : : else
130 : : {
131 [ # # ]: 0 : throw std::invalid_argument("ad::map::route::getProjectedParametricOffsetOnNeighborLane: lanes are not neighbors");
132 : : }
133 : :
134 : 2 : return offset;
135 : : }
136 : :
137 : 226 : physics::Distance calcLength(LaneInterval const &laneInterval)
138 : : {
139 [ + - + - ]: 226 : auto currentLane = lane::getLane(laneInterval.laneId);
140 [ + - + - ]: 226 : auto const resultDistance = currentLane.length * calcParametricLength(laneInterval);
141 : 452 : return resultDistance;
142 : : }
143 : :
144 : 6 : physics::Duration calcDuration(LaneInterval const &laneInterval)
145 : : {
146 [ + - + - ]: 6 : auto currentLane = lane::getLane(laneInterval.laneId);
147 [ + - + - ]: 18 : return lane::getDuration(currentLane, toParametricRange(laneInterval));
148 : : }
149 : :
150 : : enum class EdgeType
151 : : {
152 : : LEFT,
153 : : RIGHT,
154 : : LEFT_PROJECTED,
155 : : RIGHT_PROJECTED
156 : : };
157 : :
158 : 38 : template <typename LaneEdge> void getEdge(LaneInterval const &laneInterval, EdgeType edgeType, LaneEdge &outputEdge)
159 : : {
160 [ + - + - ]: 76 : auto currentLane = lane::getLane(laneInterval.laneId);
161 : :
162 [ + - + + ]: 38 : if (isRouteDirectionPositive(laneInterval))
163 : : {
164 [ + + ]: 26 : if (edgeType == EdgeType::LEFT)
165 : : {
166 [ + - + - ]: 6 : point::getParametricRange(currentLane.edgeLeft, toParametricRange(laneInterval), outputEdge, false);
167 : : }
168 [ + + ]: 20 : else if (edgeType == EdgeType::LEFT_PROJECTED)
169 : : {
170 : 7 : auto projectedInterval = laneInterval;
171 [ + - ]: 7 : projectedInterval.start = point::findNearestPointOnEdge(
172 : : currentLane.edgeLeft,
173 [ + - ]: 7 : lane::getProjectedParametricPoint(currentLane, laneInterval.start, physics::ParametricValue(0.)));
174 [ + - ]: 7 : projectedInterval.end = point::findNearestPointOnEdge(
175 : : currentLane.edgeLeft,
176 [ + - ]: 7 : lane::getProjectedParametricPoint(currentLane, laneInterval.end, physics::ParametricValue(0.)));
177 [ + - + - ]: 7 : point::getParametricRange(currentLane.edgeLeft, toParametricRange(projectedInterval), outputEdge, false);
178 : : }
179 [ + + ]: 13 : else if (edgeType == EdgeType::RIGHT)
180 : : {
181 [ + - + - ]: 6 : point::getParametricRange(currentLane.edgeRight, toParametricRange(laneInterval), outputEdge, false);
182 : : }
183 [ + - ]: 7 : else if (edgeType == EdgeType::RIGHT_PROJECTED)
184 : : {
185 : 7 : auto projectedInterval = laneInterval;
186 [ + - ]: 7 : projectedInterval.start = point::findNearestPointOnEdge(
187 : : currentLane.edgeRight,
188 [ + - ]: 7 : lane::getProjectedParametricPoint(currentLane, laneInterval.start, physics::ParametricValue(1.)));
189 [ + - ]: 7 : projectedInterval.end = point::findNearestPointOnEdge(
190 : : currentLane.edgeRight,
191 [ + - ]: 7 : lane::getProjectedParametricPoint(currentLane, laneInterval.end, physics::ParametricValue(1.)));
192 [ + - + - ]: 7 : point::getParametricRange(currentLane.edgeRight, toParametricRange(projectedInterval), outputEdge, false);
193 : : }
194 : : }
195 : : else
196 : : {
197 [ + + ]: 12 : if (edgeType == EdgeType::LEFT)
198 : : {
199 [ + - + - ]: 2 : point::getParametricRange(currentLane.edgeRight, toParametricRange(laneInterval), outputEdge, true);
200 : : }
201 [ + + ]: 10 : else if (edgeType == EdgeType::LEFT_PROJECTED)
202 : : {
203 : 4 : auto projectedInterval = laneInterval;
204 [ + - ]: 4 : projectedInterval.start = point::findNearestPointOnEdge(
205 : : currentLane.edgeRight,
206 [ + - ]: 4 : lane::getProjectedParametricPoint(currentLane, laneInterval.start, physics::ParametricValue(1.)));
207 [ + - ]: 4 : projectedInterval.end = point::findNearestPointOnEdge(
208 : : currentLane.edgeRight,
209 [ + - ]: 4 : lane::getProjectedParametricPoint(currentLane, laneInterval.end, physics::ParametricValue(1.)));
210 [ + - + - ]: 4 : point::getParametricRange(currentLane.edgeRight, toParametricRange(projectedInterval), outputEdge, true);
211 : : }
212 [ + + ]: 6 : else if (edgeType == EdgeType::RIGHT)
213 : : {
214 [ + - + - ]: 2 : point::getParametricRange(currentLane.edgeLeft, toParametricRange(laneInterval), outputEdge, true);
215 : : }
216 [ + - ]: 4 : else if (edgeType == EdgeType::RIGHT_PROJECTED)
217 : : {
218 : 4 : auto projectedInterval = laneInterval;
219 [ + - ]: 4 : projectedInterval.start = point::findNearestPointOnEdge(
220 : : currentLane.edgeLeft,
221 [ + - ]: 4 : lane::getProjectedParametricPoint(currentLane, laneInterval.start, physics::ParametricValue(0.)));
222 [ + - ]: 4 : projectedInterval.end = point::findNearestPointOnEdge(
223 : : currentLane.edgeLeft,
224 [ + - ]: 4 : lane::getProjectedParametricPoint(currentLane, laneInterval.end, physics::ParametricValue(0.)));
225 [ + - + - ]: 4 : point::getParametricRange(currentLane.edgeLeft, toParametricRange(projectedInterval), outputEdge, true);
226 : : }
227 : : }
228 : 38 : }
229 : :
230 : 2 : void getLeftEdge(LaneInterval const &laneInterval, point::ENUEdge &enuEdge)
231 : : {
232 : 2 : getEdge(laneInterval, EdgeType::LEFT, enuEdge);
233 : 2 : }
234 : :
235 : 2 : void getRightEdge(LaneInterval const &laneInterval, point::ENUEdge &enuEdge)
236 : : {
237 : 2 : getEdge(laneInterval, EdgeType::RIGHT, enuEdge);
238 : 2 : }
239 : :
240 : 9 : void getLeftProjectedEdge(LaneInterval const &laneInterval, point::ENUEdge &enuEdge)
241 : : {
242 : 9 : getEdge(laneInterval, EdgeType::LEFT_PROJECTED, enuEdge);
243 : 9 : }
244 : :
245 : 9 : void getRightProjectedEdge(LaneInterval const &laneInterval, point::ENUEdge &enuEdge)
246 : : {
247 : 9 : getEdge(laneInterval, EdgeType::RIGHT_PROJECTED, enuEdge);
248 : 9 : }
249 : :
250 : 3 : void getLeftEdge(LaneInterval const &laneInterval, point::GeoEdge &geoEdge)
251 : : {
252 : 3 : getEdge(laneInterval, EdgeType::LEFT, geoEdge);
253 : 3 : }
254 : :
255 : 3 : void getRightEdge(LaneInterval const &laneInterval, point::GeoEdge &geoEdge)
256 : : {
257 : 3 : getEdge(laneInterval, EdgeType::RIGHT, geoEdge);
258 : 3 : }
259 : :
260 : 1 : void getLeftProjectedEdge(LaneInterval const &laneInterval, point::GeoEdge &geoEdge)
261 : : {
262 : 1 : getEdge(laneInterval, EdgeType::LEFT_PROJECTED, geoEdge);
263 : 1 : }
264 : :
265 : 1 : void getRightProjectedEdge(LaneInterval const &laneInterval, point::GeoEdge &geoEdge)
266 : : {
267 : 1 : getEdge(laneInterval, EdgeType::RIGHT_PROJECTED, geoEdge);
268 : 1 : }
269 : :
270 : 3 : void getLeftEdge(LaneInterval const &laneInterval, point::ECEFEdge &ecefEdge)
271 : : {
272 : 3 : getEdge(laneInterval, EdgeType::LEFT, ecefEdge);
273 : 3 : }
274 : :
275 : 3 : void getRightEdge(LaneInterval const &laneInterval, point::ECEFEdge &ecefEdge)
276 : : {
277 : 3 : getEdge(laneInterval, EdgeType::RIGHT, ecefEdge);
278 : 3 : }
279 : :
280 : 1 : void getLeftProjectedEdge(LaneInterval const &laneInterval, point::ECEFEdge &ecefEdge)
281 : : {
282 : 1 : getEdge(laneInterval, EdgeType::LEFT_PROJECTED, ecefEdge);
283 : 1 : }
284 : :
285 : 1 : void getRightProjectedEdge(LaneInterval const &laneInterval, point::ECEFEdge &ecefEdge)
286 : : {
287 : 1 : getEdge(laneInterval, EdgeType::RIGHT_PROJECTED, ecefEdge);
288 : 1 : }
289 : :
290 : 1 : point::ENUEdge getRightENUEdge(LaneInterval const &laneInterval)
291 : : {
292 : 1 : point::ENUEdge enuEdge;
293 [ + - ]: 1 : getRightEdge(laneInterval, enuEdge);
294 : 1 : return enuEdge;
295 : : }
296 : :
297 : 2 : point::ECEFEdge getRightECEFEdge(LaneInterval const &laneInterval)
298 : : {
299 : 2 : point::ECEFEdge ecefEdge;
300 [ + - ]: 2 : getRightEdge(laneInterval, ecefEdge);
301 : 2 : return ecefEdge;
302 : : }
303 : :
304 : 2 : point::GeoEdge getRightGeoEdge(LaneInterval const &laneInterval)
305 : : {
306 : 2 : point::GeoEdge geoEdge;
307 [ + - ]: 2 : getRightEdge(laneInterval, geoEdge);
308 : 2 : return geoEdge;
309 : : }
310 : :
311 : 1 : point::ENUEdge getLeftENUEdge(LaneInterval const &laneInterval)
312 : : {
313 : 1 : point::ENUEdge enuEdge;
314 [ + - ]: 1 : getLeftEdge(laneInterval, enuEdge);
315 : 1 : return enuEdge;
316 : : }
317 : :
318 : 2 : point::ECEFEdge getLeftECEFEdge(LaneInterval const &laneInterval)
319 : : {
320 : 2 : point::ECEFEdge ecefEdge;
321 [ + - ]: 2 : getLeftEdge(laneInterval, ecefEdge);
322 : 2 : return ecefEdge;
323 : : }
324 : :
325 : 2 : point::GeoEdge getLeftGeoEdge(LaneInterval const &laneInterval)
326 : : {
327 : 2 : point::GeoEdge geoEdge;
328 [ + - ]: 2 : getLeftEdge(laneInterval, geoEdge);
329 : 2 : return geoEdge;
330 : : }
331 : :
332 : 4 : point::ENUEdge getRightProjectedENUEdge(LaneInterval const &laneInterval)
333 : : {
334 : 4 : point::ENUEdge enuEdge;
335 [ + - ]: 4 : getRightProjectedEdge(laneInterval, enuEdge);
336 : 4 : return enuEdge;
337 : : }
338 : :
339 : 0 : point::ECEFEdge getRightProjectedECEFEdge(LaneInterval const &laneInterval)
340 : : {
341 : 0 : point::ECEFEdge ecefEdge;
342 [ # # ]: 0 : getRightProjectedEdge(laneInterval, ecefEdge);
343 : 0 : return ecefEdge;
344 : : }
345 : :
346 : 0 : point::GeoEdge getRightProjectedGeoEdge(LaneInterval const &laneInterval)
347 : : {
348 : 0 : point::GeoEdge geoEdge;
349 [ # # ]: 0 : getRightProjectedEdge(laneInterval, geoEdge);
350 : 0 : return geoEdge;
351 : : }
352 : :
353 : 4 : point::ENUEdge getLeftProjectedENUEdge(LaneInterval const &laneInterval)
354 : : {
355 : 4 : point::ENUEdge enuEdge;
356 [ + - ]: 4 : getLeftProjectedEdge(laneInterval, enuEdge);
357 : 4 : return enuEdge;
358 : : }
359 : :
360 : 0 : point::ECEFEdge getLeftProjectedECEFEdge(LaneInterval const &laneInterval)
361 : : {
362 : 0 : point::ECEFEdge ecefEdge;
363 [ # # ]: 0 : getLeftProjectedEdge(laneInterval, ecefEdge);
364 : 0 : return ecefEdge;
365 : : }
366 : :
367 : 0 : point::GeoEdge getLeftProjectedGeoEdge(LaneInterval const &laneInterval)
368 : : {
369 : 0 : point::GeoEdge geoEdge;
370 [ # # ]: 0 : getLeftProjectedEdge(laneInterval, geoEdge);
371 : 0 : return geoEdge;
372 : : }
373 : :
374 : 1 : lane::GeoBorder getGeoBorder(LaneInterval const &laneInterval)
375 : : {
376 : 1 : lane::GeoBorder geoBorder;
377 [ + - ]: 1 : getLeftEdge(laneInterval, geoBorder.left);
378 [ + - ]: 1 : getRightEdge(laneInterval, geoBorder.right);
379 : 1 : return geoBorder;
380 : : }
381 : :
382 : 1 : lane::ECEFBorder getECEFBorder(LaneInterval const &laneInterval)
383 : : {
384 : 1 : lane::ECEFBorder ecefBorder;
385 [ + - ]: 1 : getLeftEdge(laneInterval, ecefBorder.left);
386 [ + - ]: 1 : getRightEdge(laneInterval, ecefBorder.right);
387 : 1 : return ecefBorder;
388 : : }
389 : :
390 : 1 : lane::ENUBorder getENUBorder(LaneInterval const &laneInterval)
391 : : {
392 : 1 : lane::ENUBorder enuBorder;
393 [ + - ]: 1 : getLeftEdge(laneInterval, enuBorder.left);
394 [ + - ]: 1 : getRightEdge(laneInterval, enuBorder.right);
395 : 1 : return enuBorder;
396 : : }
397 : :
398 : 4 : lane::ENUBorder getENUProjectedBorder(LaneInterval const &laneInterval)
399 : : {
400 : 4 : lane::ENUBorder enuBorder;
401 [ + - ]: 4 : getLeftProjectedEdge(laneInterval, enuBorder.left);
402 [ + - ]: 4 : getRightProjectedEdge(laneInterval, enuBorder.right);
403 : 4 : return enuBorder;
404 : : }
405 : :
406 : 4 : LaneInterval shortenIntervalFromBegin(LaneInterval const &laneInterval, physics::Distance const &distance)
407 : : {
408 : 4 : LaneInterval result = laneInterval;
409 [ + - + - ]: 4 : physics::ParametricValue delta(distance / lane::calcLength(laneInterval.laneId));
410 [ + - + + ]: 4 : if (isRouteDirectionPositive(laneInterval))
411 : : {
412 [ + - + - ]: 3 : result.start = std::min(laneInterval.start + delta, laneInterval.end);
413 : : }
414 : : else
415 : : {
416 [ + - + - ]: 1 : result.start = std::max(laneInterval.start - delta, laneInterval.end);
417 : : }
418 : 8 : return result;
419 : : }
420 : :
421 : 3 : LaneInterval restrictIntervalFromBegin(LaneInterval const &laneInterval, physics::Distance const &distance)
422 : : {
423 : 3 : LaneInterval result = laneInterval;
424 [ + - + - ]: 3 : physics::ParametricValue delta(distance / lane::calcLength(laneInterval.laneId));
425 [ + - + + ]: 3 : if (isRouteDirectionNegative(laneInterval))
426 : : {
427 [ + - + - ]: 1 : result.end = std::max(physics::ParametricValue(0.), laneInterval.start - delta);
428 : : }
429 : : else
430 : : {
431 [ + - + - ]: 2 : result.end = std::min(physics::ParametricValue(1.), laneInterval.start + delta);
432 : : }
433 : :
434 : 6 : return result;
435 : : }
436 : :
437 : 3 : LaneInterval extendIntervalUntilEnd(LaneInterval const &laneInterval)
438 : : {
439 : 3 : LaneInterval resultInterval = laneInterval;
440 [ + + ]: 3 : if (isDegenerated(resultInterval))
441 : : {
442 : : // nothing to be done
443 : : }
444 [ + + ]: 2 : else if (isRouteDirectionPositive(resultInterval))
445 : : {
446 : 1 : resultInterval.end = physics::ParametricValue(1.0);
447 : : }
448 : : else
449 : : {
450 : 1 : resultInterval.end = physics::ParametricValue(0.0);
451 : : }
452 : 3 : return resultInterval;
453 : : }
454 : :
455 : 4 : LaneInterval shortenIntervalFromEnd(LaneInterval const &laneInterval, physics::Distance const &distance)
456 : : {
457 : 4 : LaneInterval result = laneInterval;
458 [ + - + - ]: 4 : physics::ParametricValue delta(distance / lane::calcLength(laneInterval.laneId));
459 [ + - + + ]: 4 : if (isRouteDirectionPositive(laneInterval))
460 : : {
461 [ + - + - ]: 1 : result.end = std::max(laneInterval.end - delta, laneInterval.start);
462 : : }
463 : : else
464 : : {
465 [ + - + - ]: 3 : result.end = std::min(laneInterval.end + delta, laneInterval.start);
466 : : }
467 : 8 : return result;
468 : : }
469 : :
470 : 72 : LaneInterval extendIntervalFromStart(LaneInterval const &laneInterval, physics::Distance const &distance)
471 : : {
472 [ + - + + ]: 72 : if (isDegenerated(laneInterval))
473 : : {
474 : 1 : return laneInterval;
475 : : }
476 : :
477 : 71 : LaneInterval resultInterval = laneInterval;
478 [ + - + - ]: 71 : physics::ParametricValue offset(distance / lane::calcLength(laneInterval.laneId));
479 : :
480 [ + - + + ]: 71 : if (isRouteDirectionPositive(resultInterval))
481 : : {
482 [ + - + - ]: 42 : resultInterval.start = std::max(physics::ParametricValue(0.0), laneInterval.start - offset);
483 : : }
484 : : else
485 : : {
486 [ + - + - ]: 29 : resultInterval.start = std::min(physics::ParametricValue(1.0), laneInterval.start + offset);
487 : : }
488 : 71 : return resultInterval;
489 : : }
490 : :
491 : 72 : LaneInterval extendIntervalFromEnd(LaneInterval const &laneInterval, physics::Distance const &distance)
492 : : {
493 [ + - + + ]: 72 : if (isDegenerated(laneInterval))
494 : : {
495 : 1 : return laneInterval;
496 : : }
497 : :
498 : 71 : LaneInterval resultInterval = laneInterval;
499 [ + - + - ]: 71 : physics::ParametricValue offset(distance / lane::calcLength(laneInterval.laneId));
500 : :
501 [ + - + + ]: 71 : if (isRouteDirectionPositive(resultInterval))
502 : : {
503 [ + - + - ]: 42 : resultInterval.end = std::min(physics::ParametricValue(1.0), laneInterval.end + offset);
504 : : }
505 : : else
506 : : {
507 [ + - + - ]: 29 : resultInterval.end = std::max(physics::ParametricValue(0.0), laneInterval.end - offset);
508 : : }
509 : 71 : return resultInterval;
510 : : }
511 : :
512 : 3 : LaneInterval extendIntervalUntilStart(LaneInterval const &laneInterval)
513 : : {
514 : 3 : LaneInterval resultInterval = laneInterval;
515 [ + + ]: 3 : if (isDegenerated(resultInterval))
516 : : {
517 : : // nothing to be done
518 : : }
519 [ + + ]: 2 : else if (isRouteDirectionPositive(resultInterval))
520 : : {
521 : 1 : resultInterval.start = physics::ParametricValue(0.0);
522 : : }
523 : : else
524 : : {
525 : 1 : resultInterval.start = physics::ParametricValue(1.0);
526 : : }
527 : 3 : return resultInterval;
528 : : }
529 : :
530 : 2 : LaneInterval cutIntervalAtStart(LaneInterval const &laneInterval, physics::ParametricValue const &newIntervalStart)
531 : : {
532 : 2 : LaneInterval result = laneInterval;
533 [ + + ]: 2 : if (isWithinInterval(laneInterval, newIntervalStart))
534 : : {
535 : 1 : result.start = newIntervalStart;
536 : : }
537 : 2 : return result;
538 : : }
539 : :
540 : 8 : LaneInterval cutIntervalAtEnd(LaneInterval const &laneInterval, physics::ParametricValue const &newIntervalEnd)
541 : : {
542 : 8 : LaneInterval result = laneInterval;
543 [ + + ]: 8 : if (isWithinInterval(laneInterval, newIntervalEnd))
544 : : {
545 : 1 : result.end = newIntervalEnd;
546 : : }
547 : 8 : return result;
548 : : }
549 : :
550 : 1392 : restriction::SpeedLimitList getSpeedLimits(LaneInterval const &laneInterval)
551 : : {
552 [ + - ]: 1392 : auto lanePtr = lane::getLanePtr(laneInterval.laneId);
553 [ + - + - ]: 4176 : return getSpeedLimits(*lanePtr, toParametricRange(laneInterval));
554 : : }
555 : :
556 : 0 : void getMetricRanges(LaneInterval const &laneInterval,
557 : : physics::MetricRange &lengthRange,
558 : : physics::MetricRange &widthRange)
559 : : {
560 [ # # ]: 0 : auto lanePtr = lane::getLanePtr(laneInterval.laneId);
561 [ # # # # : 0 : if (std::fabs(laneInterval.end - laneInterval.start) == physics::ParametricValue(1.0))
# # ]
562 : : {
563 : 0 : lengthRange = lanePtr->lengthRange;
564 : 0 : widthRange = lanePtr->widthRange;
565 : : }
566 : : else
567 : : {
568 [ # # ]: 0 : auto const enuBorders = getENUProjectedBorder(laneInterval);
569 [ # # ]: 0 : auto const leftLength = calcLength(enuBorders.left);
570 [ # # ]: 0 : auto const rightLength = calcLength(enuBorders.right);
571 [ # # ]: 0 : lengthRange.minimum = std::min(leftLength, rightLength);
572 [ # # ]: 0 : lengthRange.maximum = std::max(leftLength, rightLength);
573 : :
574 [ # # # # : 0 : if ((lanePtr->widthRange.maximum - lanePtr->widthRange.minimum) <= physics::Distance(.1))
# # ]
575 : : {
576 : 0 : widthRange = lanePtr->widthRange;
577 : : }
578 : : else
579 : : {
580 : : // take the effort on with range calculation only if there is significant difference within the lane
581 [ # # ]: 0 : auto const widthRangeResult = calculateWidthRange(enuBorders.left, leftLength, enuBorders.right, rightLength);
582 : 0 : widthRange = widthRangeResult.first;
583 : : }
584 : : }
585 : 0 : }
586 : :
587 : : } // namespace route
588 : : } // namespace map
589 : : } // namespace ad
|
