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 : : * @file
10 : : */
11 : :
12 : : #pragma once
13 : :
14 : : #include <utility>
15 : : #include <vector>
16 : : #include "ad/map/point/PointOperation.hpp"
17 : : #include "ad/physics/MetricRange.hpp"
18 : : #include "ad/physics/Operation.hpp"
19 : : #include "ad/physics/ParametricRange.hpp"
20 : : #include "ad/physics/ParametricValueValidInputRange.hpp"
21 : : #include "ad/physics/RangeOperation.hpp"
22 : :
23 : : /** @brief namespace ad */
24 : : namespace ad {
25 : : /** @brief namespace map */
26 : : namespace map {
27 : : /** @brief namespace point */
28 : : namespace point {
29 : :
30 : : /**
31 : : * @brief The edge point border distance
32 : : */
33 : : physics::Distance const cEdgePointBorderDistance{0.1};
34 : :
35 : : /**
36 : : * @brief Find point nearest to the line formed by two points.
37 : : *
38 : : * @param[in] a point to search for
39 : : * @param[in] pt0 First point of the line.
40 : : * @param[in] pt1 Second point of the line.
41 : : * @returns Value of t; nearest point to a can be calculated as (1-t)*pt0+t*pt1.
42 : : */
43 : : template <typename PointType>
44 : 23422701 : physics::RatioValue findNearestPointOnEdge(PointType const &a, const PointType &pt0, const PointType &pt1)
45 : : {
46 [ + - ]: 23422701 : PointType line = pt1 - pt0;
47 [ + - ]: 23422701 : PointType pt02a = a - pt0;
48 : 23422701 : auto const divisor = vectorDotProduct(line, line);
49 [ + - + + ]: 23422701 : if (physics::Distance(divisor) > physics::Distance(0))
50 : : {
51 : 23159201 : auto const dividend = vectorDotProduct(pt02a, line);
52 : 23159201 : auto result = dividend / divisor;
53 : 23159201 : return physics::RatioValue(result);
54 : : }
55 : : else
56 : : {
57 : 263453 : return physics::RatioValue(0.5);
58 : : }
59 : : }
60 : :
61 : : /**
62 : : * @brief Find point nearest to the segment formed by two points.
63 : : *
64 : : * @param[in] a point to search for
65 : : * @param[in] pt0 First point of the segment.
66 : : * @param[in] pt1 Second point of the segment.
67 : : * @returns Value of 0<=t<=1; nearest point to this can be calculated as (1-t)*pt0+t*pt1.
68 : : *
69 : : */
70 : : template <typename PointType>
71 : 22953701 : physics::ParametricValue findNearestPointOnSegment(PointType const &a, const PointType &pt0, const PointType &pt1)
72 : : {
73 [ + - ]: 22953701 : auto const t = findNearestPointOnEdge(a, pt0, pt1);
74 [ + - + + ]: 22953701 : if (t < physics::RatioValue(0.))
75 : : {
76 : 10048700 : return physics::ParametricValue(0.);
77 : : }
78 [ + - + + ]: 12904901 : else if (t > physics::RatioValue(1.))
79 : : {
80 : 10462300 : return physics::ParametricValue(1.);
81 : : }
82 : : else
83 : : {
84 : 2442601 : return physics::ParametricValue(static_cast<double>(t));
85 : : }
86 : : }
87 : :
88 : : /**
89 : : * @brief Calculate the length of an edge
90 : : * @param[in] edge The input edge to operate on.
91 : : * @return The length of the edge
92 : : */
93 : 53344 : template <typename PointType> physics::Distance calculateEdgeLength(std::vector<PointType> const &edge)
94 : : {
95 : 53344 : physics::Distance length(0.);
96 [ + + ]: 490740 : for (auto i = 1u; i < edge.size(); ++i)
97 : : {
98 [ + - + - ]: 437396 : length += distance(edge[i - 1u], edge[i]);
99 : : }
100 : 53344 : return length;
101 : : }
102 : :
103 : : /**
104 : : * @brief Get the parametric edge points
105 : : * @param[in] edge The input edge to operate on.
106 : : * @return Vector of parametric values on the edge.
107 : : */
108 : : template <typename PointType>
109 : 16 : std::vector<physics::ParametricValue> getParametricEdgePoints(std::vector<PointType> const &edge)
110 : : {
111 : 16 : std::vector<physics::ParametricValue> resultPoints;
112 [ + - ]: 16 : resultPoints.reserve(edge.size());
113 [ + - ]: 16 : resultPoints.push_back(physics::ParametricValue(0.));
114 : 16 : physics::Distance length(0.);
115 [ + + ]: 38 : for (auto i = 1u; i < edge.size(); ++i)
116 : : {
117 [ + - + - ]: 22 : length += distance(edge[i - 1u], edge[i]);
118 [ + - ]: 22 : resultPoints.push_back(physics::ParametricValue(static_cast<double>(length)));
119 : : }
120 : :
121 [ + + ]: 38 : for (auto i = 1u; i < edge.size(); ++i)
122 : : {
123 [ + - + - ]: 22 : if (length > physics::Distance(0.))
124 : : {
125 [ + - ]: 22 : resultPoints[i] = resultPoints[i] / static_cast<double>(length);
126 : : }
127 : : }
128 : 32 : return resultPoints;
129 : : }
130 : :
131 : : /**
132 : : * @brief Calculates parametric point on the edge.
133 : : * @param[in] edge The input edge to operate on.
134 : : * @param[in] t Parameter. 0 will return first point, and 1 last point on the edge.
135 : : * @return Parametric point on the edge.
136 : : */
137 : : template <typename PointType>
138 : 5131463 : PointType getParametricPoint(std::vector<PointType> const &edge,
139 : : physics::Distance const &edgeLength,
140 : : const physics::ParametricValue &t)
141 : : {
142 [ + + ]: 5131463 : if (!edge.empty())
143 : : {
144 : 5131463 : physics::Distance length(0);
145 [ + - ]: 5131463 : physics::Distance length_t = edgeLength * t;
146 [ + + ]: 25519503 : for (size_t i = 0; i < edge.size() - 1; i++)
147 : : {
148 : 25519303 : const auto &pt0 = edge[i];
149 : 25519303 : const auto &pt1 = edge[i + 1];
150 [ + - ]: 25519303 : auto d = distance(pt0, pt1);
151 [ + - + + ]: 25519303 : if (d != physics::Distance(0.))
152 : : {
153 [ + - ]: 25519003 : auto length_1 = length + d;
154 [ + - + + ]: 25519003 : if (length_1 >= length_t)
155 : : {
156 [ + - ]: 5131213 : auto d_t = length_t - length;
157 [ + - ]: 5131213 : physics::ParametricValue tt(d_t / d);
158 [ + - ]: 5131213 : return vectorInterpolate(pt0, pt1, tt);
159 : : }
160 : : else
161 : : {
162 : 20387800 : length = length_1;
163 : : }
164 : : }
165 : : }
166 : 252 : return edge.back();
167 : : }
168 : : else
169 : : {
170 : 1 : return PointType();
171 : : }
172 : : }
173 : :
174 : : /**
175 : : * @brief Calculates parametric point on the edge.
176 : : * @param[in] edge The input edge to operate on.
177 : : * @param[in] t Parameter. 0 will return first point, and 1 last point on the edge.
178 : : * @return Parametric point on the edge.
179 : : */
180 : : template <typename PointType>
181 : 0 : PointType getParametricPoint(std::vector<PointType> const &edge, const physics::ParametricValue &t)
182 : : {
183 [ # # ]: 0 : return getParametricPoint(edge, calculateEdgeLength(edge), t);
184 : : }
185 : :
186 : : /**
187 : : * @brief Generates sub-edge for given range.
188 : : * @param[in] edge The input edge to operate on.
189 : : * @param[in] trange Specifies parametric range.
190 : : * @return Sub-geometry.
191 : : */
192 : : template <typename PointType>
193 : 5638 : std::vector<PointType> getParametricRange(std::vector<PointType> const &edge,
194 : : physics::Distance const &edgeLength,
195 : : const physics::ParametricRange &trange)
196 : : {
197 : : typedef std::vector<PointType> EdgeType;
198 [ + - ]: 5638 : if (!edge.empty())
199 : : {
200 : 5638 : EdgeType pts;
201 : 5638 : physics::Distance length(0);
202 [ + - ]: 5638 : physics::Distance length_t_start = edgeLength * trange.minimum;
203 : 5638 : size_t i = 0;
204 [ + - + - : 5654 : for (; i < edge.size() - 1 && pts.empty(); i++)
+ - ]
205 : : {
206 : 5654 : const PointType &pt0 = edge[i];
207 : 5654 : const PointType &pt1 = edge[i + 1];
208 [ + - ]: 5654 : physics::Distance d = distance(pt0, pt1);
209 [ + - ]: 5654 : physics::Distance length_1 = length + d;
210 [ + - + + ]: 5654 : if (length_1 >= length_t_start)
211 : : {
212 [ + - + + ]: 5638 : if (d != physics::Distance(0))
213 : : {
214 [ + - ]: 5632 : physics::Distance d_t = length_t_start - length;
215 [ + - ]: 5632 : physics::ParametricValue tt(d_t / d);
216 [ + - ]: 5632 : PointType pt_start = vectorInterpolate(pt0, pt1, tt);
217 [ + - ]: 5632 : pts.push_back(pt_start);
218 : : }
219 : : else
220 : : {
221 [ + - ]: 6 : pts.push_back(pt1);
222 : : }
223 : 5638 : break;
224 : : }
225 : : else
226 : : {
227 : 16 : length = length_1;
228 : : }
229 : : }
230 [ + - ]: 5638 : if (!pts.empty())
231 : : {
232 [ + - ]: 5638 : physics::Distance length_t_end = edgeLength * trange.maximum;
233 [ + - ]: 54599 : for (; i < edge.size() - 1; i++)
234 : : {
235 : 54599 : const PointType &pt0 = edge[i];
236 : 54599 : const PointType &pt1 = edge[i + 1];
237 [ + - ]: 54599 : physics::Distance d = distance(pt0, pt1);
238 [ + - ]: 54599 : physics::Distance length_1 = length + d;
239 [ + - + + ]: 54599 : if (length_1 >= length_t_end)
240 : : {
241 [ + - + + ]: 5638 : if (d != physics::Distance(0))
242 : : {
243 [ + - ]: 5632 : physics::Distance d_t = length_t_end - length;
244 [ + - ]: 5632 : physics::ParametricValue tt(d_t / d);
245 [ + - ]: 5632 : PointType pt_end = vectorInterpolate(pt0, pt1, tt);
246 [ + - ]: 5632 : pts.push_back(pt_end);
247 : : }
248 : : else
249 : : {
250 [ + - ]: 6 : pts.push_back(pt1);
251 : : }
252 : 5638 : return pts;
253 : : }
254 : : else
255 : : {
256 [ + - ]: 48961 : pts.push_back(pt1);
257 : 48961 : length = length_1;
258 : : }
259 : : }
260 [ # # ]: 0 : pts.push_back(edge.back());
261 : 0 : return pts;
262 : : }
263 : : }
264 : 0 : return EdgeType();
265 : : }
266 : :
267 : : /**
268 : : * @brief Generates sub-edge for given range.
269 : : * @param[in] edge The input edge to operate on.
270 : : * @param[in] trange Specifies parametric range.
271 : : * @return Sub-geometry.
272 : : */
273 : : template <typename PointType>
274 : 5590 : std::vector<PointType> getParametricRange(std::vector<PointType> const &edge, const physics::ParametricRange &trange)
275 : : {
276 [ + - ]: 11180 : return getParametricRange(edge, calculateEdgeLength(edge), trange);
277 : : }
278 : :
279 : : /**
280 : : * @brief Finds point on geometry nearest to given point.
281 : : * @param[in] pt Point of interest.
282 : : * @returns Parametric point on geometry nearest to the pt.
283 : : * Can be invalid (if pt is Invalid(), geometry is empty etc.).
284 : : */
285 : : template <typename PointType>
286 : : physics::ParametricValue
287 : 2630711 : findNearestPointOnEdge(std::vector<PointType> const &edge, physics::Distance const &edgeLength, const PointType &pt)
288 : : {
289 [ + - ]: 2630711 : if (isValid(pt))
290 : : {
291 [ - + ]: 2630711 : if (edge.size() == 0)
292 : : {
293 : 0 : return physics::ParametricValue();
294 : : }
295 [ + - + - : 2630711 : else if ((edge.size() == 1) || (edgeLength == physics::Distance(0.)))
+ + + + ]
296 : : {
297 : 126 : return physics::ParametricValue(0);
298 : : }
299 : : else
300 : : {
301 [ + - ]: 2630581 : physics::ParametricValue t_one = findNearestPointOnSegment(pt, edge[0], edge[1]);
302 [ + - ]: 2630581 : PointType pt_nearest = vectorInterpolate(edge[0], edge[1], t_one);
303 [ + - ]: 2630581 : physics::Distance d_nearest = distance(pt, pt_nearest);
304 [ + - ]: 2630581 : physics::Distance offset_nearest = distance(pt_nearest, edge[0]);
305 : 2630581 : physics::Distance running_offset = physics::Distance(0);
306 [ + + ]: 22953701 : for (size_t i = 1; i + 1 < edge.size(); i++)
307 : : {
308 [ + - ]: 20323100 : physics::ParametricValue t = findNearestPointOnSegment(pt, edge[i], edge[i + 1]);
309 [ + - ]: 20323100 : PointType pt_candidate = vectorInterpolate(edge[i], edge[i + 1], t);
310 [ + - ]: 20323100 : physics::Distance d = distance(pt_candidate, pt);
311 [ + - + - ]: 20323100 : running_offset += distance(edge[i - 1], edge[i]);
312 [ + - + + ]: 20323100 : if (d < d_nearest)
313 : : {
314 : 10115200 : pt_nearest = pt_candidate;
315 : 10115200 : d_nearest = d;
316 [ + - + - ]: 10115200 : offset_nearest = running_offset + distance(pt_nearest, edge[i]);
317 : : }
318 : : }
319 [ + - ]: 2630581 : return physics::ParametricValue(offset_nearest / edgeLength);
320 : : }
321 : : }
322 : : else
323 : : {
324 : 0 : return physics::ParametricValue();
325 : : }
326 : : }
327 : :
328 : : /**
329 : : * @brief Find the nearest point on an edge
330 : : * @param[in] edge The input edge to operate on.
331 : : * @param[in] pt Point of interest.
332 : : * @returns Parametric point on geometry nearest to the pt.
333 : : * Can be invalid (if pt is Invalid(), geometry is empty etc.).
334 : : */
335 : : template <typename PointType>
336 : : physics::ParametricValue findNearestPointOnEdge(std::vector<PointType> const &edge, const PointType &pt)
337 : : {
338 : : return findNearestPointOnEdge(edge, calculateEdgeLength(edge), pt);
339 : : }
340 : :
341 : : /**
342 : : * @brief Calculate the width range and the average width of a pair of edges
343 : : *
344 : : * At maximum 100 supporting points are taken into account to calculate this.
345 : : *
346 : : * @param[in] leftEdge the left-hand border edge as basis for the calculation
347 : : * @param[in] leftEdgeLength the length of the left-hand border edge
348 : : * @param[in] rightEdge the right-hand border edge as basis for the calculation
349 : : * @param[in] rightEdgeLength the length of the left-hand border edge
350 : : *
351 : : * @returns pair containing the range of the width and the average width between the two input edges.
352 : : */
353 : : template <typename PointType>
354 : 23501 : std::pair<physics::MetricRange, physics::Distance> calculateWidthRange(std::vector<PointType> const &edgeLeft,
355 : : physics::Distance const &edgeLeftLength,
356 : : std::vector<PointType> const &edgeRight,
357 : : physics::Distance const &edgeRightLength)
358 : : {
359 [ + - ]: 23501 : physics::MetricRange widthRange;
360 : 23501 : widthRange.minimum = std::numeric_limits<physics::Distance>::max();
361 : 23501 : widthRange.maximum = physics::Distance(0.);
362 : 23501 : physics::Distance widthSum(0.);
363 : 23501 : size_t widthSumCount(0u);
364 : 23501 : physics::ParametricValue parametricStepsize(0.5);
365 [ + - + - ]: 23501 : auto length = (edgeLeftLength + edgeRightLength) * 0.5;
366 [ + - + + ]: 23501 : if (length > physics::Distance(1.))
367 : : {
368 : 22424 : parametricStepsize
369 [ + - ]: 22424 : = std::max(physics::ParametricValue(0.01), physics::ParametricValue(1. / static_cast<double>(length)));
370 : : }
371 : 23501 : physics::ParametricValue longitudinalOffset(0.);
372 : 23501 : bool endReached = false;
373 : 815659 : do
374 : : {
375 [ + - ]: 839160 : auto const pt0 = getParametricPoint(edgeLeft, edgeLeftLength, longitudinalOffset);
376 [ + - ]: 839160 : auto const pt1 = getParametricPoint(edgeRight, edgeRightLength, longitudinalOffset);
377 [ + - + - : 839160 : if (!isValid(pt1) || !isValid(pt1))
+ - - + -
+ ]
378 : : {
379 [ # # # # ]: 0 : return std::make_pair(physics::MetricRange(), physics::Distance());
380 : : }
381 [ + - ]: 839160 : auto const centerPoint = point::vectorInterpolate(pt0, pt1, physics::ParametricValue(0.5));
382 [ + - - + ]: 839160 : if (!isValid(centerPoint))
383 : : {
384 [ # # # # ]: 0 : return std::make_pair(physics::MetricRange(), physics::Distance());
385 : : }
386 [ + - ]: 839160 : auto const longTLeft = findNearestPointOnEdge(edgeLeft, edgeLeftLength, centerPoint);
387 [ + - ]: 839160 : auto const longTRight = findNearestPointOnEdge(edgeRight, edgeRightLength, centerPoint);
388 [ + - + - : 839160 : if (!isRangeValid(longTLeft) || !isRangeValid(longTRight))
+ - - + -
+ ]
389 : : {
390 [ # # # # ]: 0 : return std::make_pair(physics::MetricRange(), physics::Distance());
391 : : }
392 [ + - ]: 839160 : auto const pointOnLeftEdge = getParametricPoint(edgeLeft, edgeLeftLength, longTLeft);
393 [ + - ]: 839160 : auto const pointOnRightEdge = getParametricPoint(edgeRight, edgeRightLength, longTRight);
394 [ + - + - : 839160 : if (!isValid(pointOnLeftEdge) || !isValid(pointOnRightEdge))
+ - - + -
+ ]
395 : : {
396 [ # # # # ]: 0 : return std::make_pair(physics::MetricRange(), physics::Distance());
397 : : }
398 [ + - ]: 839160 : physics::Distance const width = distance(pointOnLeftEdge, pointOnRightEdge);
399 [ + - ]: 839160 : unionRangeWith(widthRange, width);
400 [ + - ]: 839160 : widthSum += width;
401 : 839160 : widthSumCount++;
402 [ + - + + ]: 839160 : if (longitudinalOffset < physics::ParametricValue(1.))
403 : : {
404 [ + - ]: 815659 : longitudinalOffset += parametricStepsize;
405 [ + - + + ]: 815659 : if (longitudinalOffset > physics::ParametricValue(1.))
406 : : {
407 : 18804 : longitudinalOffset = physics::ParametricValue(1.);
408 : : }
409 : : }
410 : : else
411 : : {
412 : 23501 : endReached = true;
413 : : }
414 [ + + ]: 839160 : } while (!endReached);
415 : :
416 [ + - ]: 23501 : auto averageWidth = (widthSum / static_cast<double>(widthSumCount));
417 : :
418 [ + - ]: 23501 : return std::make_pair(widthRange, averageWidth);
419 : : }
420 : :
421 : : /**
422 : : * @brief Get an edge between the two given border edges with corresponding lateralAlignment
423 : : *
424 : : * The left and right edges are usually the borders of some road section.
425 : : * This function then calculates a new edge in between two other edges providing e.g. the center edge
426 : : * (lateralAlgignment=0.5)
427 : : * or edge with other lateral shift.
428 : : *
429 : : * @param[in] leftEdge the left-hand border edge as basis for the calculation
430 : : * @param[in] leftEdgeLength the length of the left-hand border edge
431 : : * @param[in] rightEdge the right-hand border edge as basis for the calculation
432 : : * @param[in] rightEdgeLength the length of the left-hand border edge
433 : : * @param[in] lateralAlignment the lateral alignment as TParam [0.;1.] used to calculate the resulting edge.
434 : : * The lateral alignment is relative to the left edge. If lateralAlignment is 1., the left edge is returned,
435 : : * if lateralAlignment is 0., the right edge is returned
436 : : *
437 : : * @throws std::invalid_argument if the lateralAlignment parameter is smaller than 0. or larger than 1.
438 : : */
439 : : template <typename PointType>
440 : 2 : std::vector<PointType> getLateralAlignmentEdge(std::vector<PointType> const &leftEdge,
441 : : physics::Distance const &leftEdgeLength,
442 : : std::vector<PointType> const &rightEdge,
443 : : physics::Distance const &rightEdgeLength,
444 : : physics::ParametricValue const lateralAlignment)
445 : : {
446 : : typedef std::vector<PointType> EdgeType;
447 : :
448 [ + - - + ]: 2 : if (!withinValidInputRange(lateralAlignment))
449 : : {
450 : : throw std::invalid_argument("ad::map::point::getLateralAlignmentEdge()"
451 [ # # ]: 0 : " the given lateralAlignment is out of range");
452 : : }
453 : :
454 : : EdgeType const *primary;
455 : : EdgeType const *secondary;
456 : 2 : physics::Distance primaryLength;
457 : 2 : physics::Distance secondaryLength;
458 : 2 : physics::ParametricValue lateralOffset = lateralAlignment;
459 [ + + ]: 2 : if (leftEdge.size() > rightEdge.size())
460 : : {
461 : 1 : primary = &leftEdge;
462 : 1 : primaryLength = leftEdgeLength;
463 : 1 : secondary = &rightEdge;
464 : 1 : secondaryLength = rightEdgeLength;
465 [ + - ]: 1 : lateralOffset = physics::ParametricValue(1.) - lateralAlignment;
466 : : }
467 : : else
468 : : {
469 : 1 : primary = &rightEdge;
470 : 1 : primaryLength = rightEdgeLength;
471 : 1 : secondary = &leftEdge;
472 : 1 : secondaryLength = leftEdgeLength;
473 : : }
474 : :
475 [ + - ]: 4 : std::vector<physics::ParametricValue> const primaryParametric = getParametricEdgePoints(*primary);
476 : 2 : EdgeType alignmentEdge;
477 [ + - ]: 2 : alignmentEdge.reserve(primaryParametric.size());
478 [ + + ]: 7 : for (size_t i = 0; i < primaryParametric.size(); i++)
479 : : {
480 [ + - ]: 5 : PointType const &pri = primary->at(i);
481 [ + - ]: 5 : PointType const sec = getParametricPoint(*secondary, secondaryLength, primaryParametric[i]);
482 [ + - ]: 5 : PointType const alignmentPoint = vectorInterpolate(pri, sec, lateralOffset);
483 [ + - ]: 5 : alignmentEdge.push_back(alignmentPoint);
484 : : }
485 : 4 : return alignmentEdge;
486 : : }
487 : :
488 : : /**
489 : : * @brief Get an edge between the two given border edges with corresponding lateralAlignment
490 : : *
491 : : * The left and right edges are usually the borders of some road section.
492 : : * This function then calculates a new edge in between two other edges providing e.g. the center edge
493 : : * (lateralAlgignment=0.5)
494 : : * or edge with other lateral shift.
495 : : *
496 : : * Note: if the length of the edges are already know, the overloaded getLateralAlignmentEdge() function can be called.
497 : : *
498 : : * @param[in] leftEdge the left-hand border edge as basis for the calculation
499 : : * @param[in] rightEdge the right-hand border edge as basis for the calculation
500 : : * @param[in] lateralAlignment the lateral alignment as TParam [0.;1.] used to calculate the resulting edge.
501 : : * The lateral alignment is relative to the left edge. If lateralAlignment is 1., the left edge is returned,
502 : : * if lateralAlignment is 0., the right edge is returned
503 : : *
504 : : * @throws std::invalid_argument if the lateralAlignment parameter is smaller than 0. or larger than 1.
505 : : */
506 : : template <typename PointType>
507 : 1 : std::vector<PointType> getLateralAlignmentEdge(std::vector<PointType> const &leftEdge,
508 : : std::vector<PointType> const &rightEdge,
509 : : physics::ParametricValue const lateralAlignment)
510 : : {
511 : : return getLateralAlignmentEdge(
512 [ + - + - ]: 2 : leftEdge, calculateEdgeLength(leftEdge), rightEdge, calculateEdgeLength(rightEdge), lateralAlignment);
513 : : }
514 : :
515 : : /**
516 : : * @brief get a normalized vector representing the edge direction at edge start
517 : : *
518 : : * If the number of edge points is <= 1, zero is returned.
519 : : * If the two start points of the edge are too close to each other, a third point is used if possible to increase
520 : : * the accuracy.
521 : : */
522 : 8 : template <typename PointType> PointType getEdgeStartDirectionalVector(std::vector<PointType> const edge)
523 : : {
524 [ - + ]: 8 : if (edge.size() <= 1u)
525 : : {
526 [ # # ]: 0 : return PointType();
527 : : }
528 : :
529 [ + - ]: 8 : auto edgeStartVec = edge[1u] - edge[0u];
530 : : // in case the end points are too near to each other we take another point into account if possible
531 [ + - + - : 8 : if ((vectorLength(edgeStartVec) < cEdgePointBorderDistance) && (edge.size() > 2u))
- + - - -
+ ]
532 : : {
533 [ # # ]: 0 : edgeStartVec = edge[2u] - edge[0u];
534 : : }
535 [ + - ]: 8 : return vectorNorm(edgeStartVec);
536 : : }
537 : :
538 : : /**
539 : : * @brief get a normalized vector representing the edge direction at edge end
540 : : *
541 : : * If the number of edge points is <= 1, zero is returned.
542 : : * If the two end points of the edge are too close to each other, a third point is used if possible to increase
543 : : * the accuracy.
544 : : */
545 : 8 : template <typename PointType> PointType getEdgeEndDirectionalVector(std::vector<PointType> const edge)
546 : : {
547 [ - + ]: 8 : if (edge.size() <= 1u)
548 : : {
549 [ # # ]: 0 : return PointType();
550 : : }
551 : :
552 [ + - ]: 8 : auto edgeEndVec = edge[edge.size() - 1u] - edge[edge.size() - 2u];
553 : : // in case the end points are too near to each other we take another point into account if possible
554 [ + - + - : 8 : if ((vectorLength(edgeEndVec) < cEdgePointBorderDistance) && (edge.size() > 2u))
- + - - -
+ ]
555 : : {
556 [ # # ]: 0 : edgeEndVec = edge[edge.size() - 1u] - edge[edge.size() - 3u];
557 : : }
558 [ + - ]: 8 : return vectorNorm(edgeEndVec);
559 : : }
560 : :
561 : : } // namespace point
562 : : } // namespace map
563 : : } // namespace ad
|
