Branch data Line data Source code
1 : : // Copyright 2017-2021 the Autoware Foundation
2 : : //
3 : : // Licensed under the Apache License, Version 2.0 (the "License");
4 : : // you may not use this file except in compliance with the License.
5 : : // You may obtain a copy of the License at
6 : : //
7 : : // http://www.apache.org/licenses/LICENSE-2.0
8 : : //
9 : : // Unless required by applicable law or agreed to in writing, software
10 : : // distributed under the License is distributed on an "AS IS" BASIS,
11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 : : // See the License for the specific language governing permissions and
13 : : // limitations under the License.
14 : : //
15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : : /// \file
17 : : /// \brief This file includes common functionality for 2D geometry, such as dot products
18 : :
19 : : #ifndef GEOMETRY__COMMON_2D_HPP_
20 : : #define GEOMETRY__COMMON_2D_HPP_
21 : :
22 : : #include <common/types.hpp>
23 : : #include <cmath>
24 : : #include <limits>
25 : : #include <stdexcept>
26 : :
27 : : #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
28 : : #include "geometry/interval.hpp"
29 : :
30 : : using autoware::common::types::float32_t;
31 : : using autoware::common::types::bool8_t;
32 : :
33 : : namespace comparison = autoware::common::helper_functions::comparisons;
34 : :
35 : : namespace autoware
36 : : {
37 : : namespace common
38 : : {
39 : : namespace geometry
40 : : {
41 : :
42 : : /// \brief Temporary namespace for point adapter methods, for use with nonstandard point types
43 : : namespace point_adapter
44 : : {
45 : : /// \brief Gets the x value for a point
46 : : /// \return The x value of the point
47 : : /// \param[in] pt The point
48 : : /// \tparam PointT The point type
49 : : template<typename PointT>
50 : 0 : inline auto x_(const PointT & pt)
51 : : {
52 : 0 : return pt.x;
53 : : }
54 : : /// \brief Gets the x value for a TrajectoryPoint message
55 : : /// \return The x value of the point
56 : : /// \param[in] pt The point
57 : : inline auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
58 : : {
59 : : return pt.pose.position.x;
60 : : }
61 : : /// \brief Gets the y value for a point
62 : : /// \return The y value of the point
63 : : /// \param[in] pt The point
64 : : /// \tparam PointT The point type
65 : : template<typename PointT>
66 : 0 : inline auto y_(const PointT & pt)
67 : : {
68 : 0 : return pt.y;
69 : : }
70 : : /// \brief Gets the y value for a TrajectoryPoint message
71 : : /// \return The y value of the point
72 : : /// \param[in] pt The point
73 : : inline auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
74 : : {
75 : : return pt.pose.position.y;
76 : : }
77 : : /// \brief Gets the z value for a point
78 : : /// \return The z value of the point
79 : : /// \param[in] pt The point
80 : : /// \tparam PointT The point type
81 : : template<typename PointT>
82 : 0 : inline auto z_(const PointT & pt)
83 : : {
84 : 0 : return pt.z;
85 : : }
86 : : /// \brief Gets the z value for a TrajectoryPoint message
87 : : /// \return The z value of the point
88 : : /// \param[in] pt The point
89 : : inline auto z_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
90 : : {
91 : : return pt.pose.position.z;
92 : : }
93 : : /// \brief Gets a reference to the x value for a point
94 : : /// \return A reference to the x value of the point
95 : : /// \param[in] pt The point
96 : : /// \tparam PointT The point type
97 : : template<typename PointT>
98 : 0 : inline auto & xr_(PointT & pt)
99 : : {
100 : 0 : return pt.x;
101 : : }
102 : : /// \brief Gets a reference to the x value for a TrajectoryPoint
103 : : /// \return A reference to the x value of the TrajectoryPoint
104 : : /// \param[in] pt The TrajectoryPoint
105 : : inline auto & xr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
106 : : {
107 : : return pt.pose.position.x;
108 : : }
109 : : /// \brief Gets a reference to the y value for a point
110 : : /// \return A reference to The y value of the point
111 : : /// \param[in] pt The point
112 : : /// \tparam PointT The point type
113 : : template<typename PointT>
114 : 0 : inline auto & yr_(PointT & pt)
115 : : {
116 : 0 : return pt.y;
117 : : }
118 : : /// \brief Gets a reference to the y value for a TrajectoryPoint
119 : : /// \return A reference to the y value of the TrajectoryPoint
120 : : /// \param[in] pt The TrajectoryPoint
121 : : inline auto & yr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
122 : : {
123 : : return pt.pose.position.y;
124 : : }
125 : : /// \brief Gets a reference to the z value for a point
126 : : /// \return A reference to the z value of the point
127 : : /// \param[in] pt The point
128 : : /// \tparam PointT The point type
129 : : template<typename PointT>
130 : : inline auto & zr_(PointT & pt)
131 : : {
132 : : return pt.z;
133 : : }
134 : : /// \brief Gets a reference to the z value for a TrajectoryPoint
135 : : /// \return A reference to the z value of the TrajectoryPoint
136 : : /// \param[in] pt The TrajectoryPoint
137 : : inline auto & zr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
138 : : {
139 : : return pt.pose.position.z;
140 : : }
141 : : } // namespace point_adapter
142 : :
143 : : namespace details
144 : : {
145 : : // Return the next iterator, cycling back to the beginning of the list if you hit the end
146 : : template<typename IT>
147 : 0 : IT circular_next(const IT begin, const IT end, const IT current) noexcept
148 : : {
149 : 0 : auto next = std::next(current);
150 : 0 : if (end == next) {
151 : 0 : next = begin;
152 : : }
153 : 0 : return next;
154 : : }
155 : :
156 : : } // namespace details
157 : :
158 : :
159 : : /// \tparam T1, T2, T3 point type. Must have point adapters defined or have float members x and y
160 : : /// \brief compute whether line segment rp is counter clockwise relative to line segment qp
161 : : /// \param[in] pt shared point for both line segments
162 : : /// \param[in] r point to check if it forms a ccw angle
163 : : /// \param[in] q reference point
164 : : /// \return whether angle formed is ccw. Three collinear points is considered ccw
165 : : template<typename T1, typename T2, typename T3>
166 : 0 : inline auto ccw(const T1 & pt, const T2 & q, const T3 & r)
167 : : {
168 : : using point_adapter::x_;
169 : : using point_adapter::y_;
170 : :
171 : 0 : return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F;
172 : : }
173 : :
174 : : /// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y
175 : : /// \brief compute p x q = p1 * q2 - p2 * q1
176 : : /// \param[in] pt first point
177 : : /// \param[in] q second point
178 : : /// \return 2d cross product
179 : : template<typename T1, typename T2>
180 : 0 : inline auto cross_2d(const T1 & pt, const T2 & q)
181 : : {
182 : : using point_adapter::x_;
183 : : using point_adapter::y_;
184 : 0 : return (x_(pt) * y_(q)) - (y_(pt) * x_(q));
185 : : }
186 : :
187 : : /// \tparam T1, T2 point type. Must have point adapters defined or have float members x and y
188 : : /// \brief compute p * q = p1 * q1 + p2 * q2
189 : : /// \param[in] pt first point
190 : : /// \param[in] q second point
191 : : /// \return 2d scalar dot product
192 : : template<typename T1, typename T2>
193 : 0 : inline auto dot_2d(const T1 & pt, const T2 & q)
194 : : {
195 : : using point_adapter::x_;
196 : : using point_adapter::y_;
197 : 0 : return (x_(pt) * x_(q)) + (y_(pt) * y_(q));
198 : : }
199 : :
200 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
201 : : /// \brief Compute the 2d difference between two points, p - q
202 : : /// \param[in] p The left hand side
203 : : /// \param[in] q The right hand side
204 : : /// \return A point with the difference in the x and y fields, all other fields are default
205 : : /// initialized
206 : : template<typename T>
207 : 0 : T minus_2d(const T & p, const T & q)
208 : : {
209 : 0 : T r;
210 : : using point_adapter::x_;
211 : : using point_adapter::y_;
212 : 0 : point_adapter::xr_(r) = x_(p) - x_(q);
213 : 0 : point_adapter::yr_(r) = y_(p) - y_(q);
214 : 0 : return r;
215 : : }
216 : :
217 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
218 : : /// \brief The unary minus or negation operator applied to a single point's 2d fields
219 : : /// \param[in] p The left hand side
220 : : /// \return A point with the negation in the x and y fields, all other fields are default
221 : : /// initialized
222 : : template<typename T>
223 : 0 : T minus_2d(const T & p)
224 : : {
225 : 0 : T r;
226 : 0 : point_adapter::xr_(r) = -point_adapter::x_(p);
227 : 0 : point_adapter::yr_(r) = -point_adapter::y_(p);
228 : 0 : return r;
229 : : }
230 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
231 : : /// \brief The 2d addition operation, p + q
232 : : /// \param[in] p The left hand side
233 : : /// \param[in] q The right hand side
234 : : /// \return A point with the sum in the x and y fields, all other fields are default
235 : : /// initialized
236 : : template<typename T>
237 : 0 : T plus_2d(const T & p, const T & q)
238 : : {
239 : 0 : T r;
240 : : using point_adapter::x_;
241 : : using point_adapter::y_;
242 : 0 : point_adapter::xr_(r) = x_(p) + x_(q);
243 : 0 : point_adapter::yr_(r) = y_(p) + y_(q);
244 : 0 : return r;
245 : : }
246 : :
247 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
248 : : /// \brief The scalar multiplication operation, p * a
249 : : /// \param[in] p The point value
250 : : /// \param[in] a The scalar value
251 : : /// \return A point with the scaled x and y fields, all other fields are default
252 : : /// initialized
253 : : template<typename T>
254 : 0 : T times_2d(const T & p, const float32_t a)
255 : : {
256 : 0 : T r;
257 : 0 : point_adapter::xr_(r) = static_cast<float32_t>(point_adapter::x_(p)) * a;
258 : 0 : point_adapter::yr_(r) = static_cast<float32_t>(point_adapter::y_(p)) * a;
259 : 0 : return r;
260 : : }
261 : :
262 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
263 : : /// \brief solve p + t * u = q + s * v
264 : : /// Ref: https://stackoverflow.com/questions/563198/
265 : : /// whats-the-most-efficent-way-to-calculate-where-two-line-segments-intersect
266 : : /// \param[in] pt anchor point of first line
267 : : /// \param[in] u direction of first line
268 : : /// \param[in] q anchor point of second line
269 : : /// \param[in] v direction of second line
270 : : /// \return intersection point
271 : : /// \throw std::runtime_error if lines are (nearly) collinear or parallel
272 : : template<typename T>
273 : 0 : inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v)
274 : : {
275 : 0 : const float32_t num = cross_2d(minus_2d(pt, q), u);
276 : 0 : float32_t den = cross_2d(v, u);
277 : 0 : constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
278 : 0 : if (fabsf(den) < FEPS) {
279 : 0 : if (fabsf(num) < FEPS) {
280 : : // collinear case, anything is ok
281 : 0 : den = 1.0F;
282 : : } else {
283 : : // parallel case, no valid output
284 : : throw std::runtime_error(
285 : 0 : "intersection_2d: no unique solution (either collinear or parallel)");
286 : : }
287 : : }
288 : 0 : return plus_2d(q, times_2d(v, num / den));
289 : : }
290 : :
291 : :
292 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
293 : : /// \brief rotate point given precomputed sin and cos
294 : : /// \param[inout] pt point to rotate
295 : : /// \param[in] cos_th precomputed cosine value
296 : : /// \param[in] sin_th precompined sine value
297 : : template<typename T>
298 : 0 : inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th)
299 : : {
300 : 0 : const float32_t x = point_adapter::x_(pt);
301 : 0 : const float32_t y = point_adapter::y_(pt);
302 : 0 : point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y);
303 : 0 : point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y);
304 : 0 : }
305 : :
306 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
307 : : /// \brief rotate by radian angle th in z direction with ccw positive
308 : : /// \param[in] pt reference point to rotate
309 : : /// \param[in] th_rad angle by which to rotate point
310 : : /// \return rotated point
311 : : template<typename T>
312 : : inline T rotate_2d(const T & pt, const float32_t th_rad)
313 : : {
314 : : T q(pt); // It's reasonable to expect a copy constructor
315 : : const float32_t s = sinf(th_rad);
316 : : const float32_t c = cosf(th_rad);
317 : : rotate_2d(q, c, s);
318 : : return q;
319 : : }
320 : :
321 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
322 : : /// \brief compute q s.t. p T q, or p * q = 0
323 : : /// This is the equivalent of a 90 degree ccw rotation
324 : : /// \param[in] pt point to get normal point of
325 : : /// \return point normal to p (unnormalized)
326 : : template<typename T>
327 : 0 : inline T get_normal(const T & pt)
328 : : {
329 : 0 : T q(pt);
330 : 0 : point_adapter::xr_(q) = -point_adapter::y_(pt);
331 : 0 : point_adapter::yr_(q) = point_adapter::x_(pt);
332 : 0 : return q;
333 : : }
334 : :
335 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
336 : : /// \brief get magnitude of x and y components:
337 : : /// \param[in] pt point to get magnitude of
338 : : /// \return magitude of x and y components together
339 : : template<typename T>
340 : 0 : inline auto norm_2d(const T & pt)
341 : : {
342 : 0 : return sqrtf(static_cast<float32_t>(dot_2d(pt, pt)));
343 : : }
344 : :
345 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
346 : : /// \brief Compute the closest point on line segment p-q to point r
347 : : /// Based on equations from https://stackoverflow.com/a/1501725 and
348 : : /// http://paulbourke.net/geometry/pointlineplane/
349 : : /// \param[in] p First point defining the line segment
350 : : /// \param[in] q Second point defining the line segment
351 : : /// \param[in] r Reference point to find the closest point to
352 : : /// \return Closest point on line segment p-q to point r
353 : : template<typename T>
354 : 0 : inline T closest_segment_point_2d(const T & p, const T & q, const T & r)
355 : : {
356 : 0 : const T qp = minus_2d(q, p);
357 : 0 : const float32_t len2 = static_cast<float32_t>(dot_2d(qp, qp));
358 : 0 : T ret = p;
359 : 0 : if (len2 > std::numeric_limits<float32_t>::epsilon()) {
360 : 0 : const Interval_f unit_interval(0.0f, 1.0f);
361 : 0 : const float32_t val = static_cast<float32_t>(dot_2d(minus_2d(r, p), qp)) / len2;
362 : 0 : const float32_t t = Interval_f::clamp_to(unit_interval, val);
363 : 0 : ret = plus_2d(p, times_2d(qp, t));
364 : : }
365 : 0 : return ret;
366 : : }
367 : : //
368 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
369 : : /// \brief Compute the closest point on the line going through p-q to point r
370 : : // Obtained by simplifying closest_segment_point_2d.
371 : : /// \param[in] p First point defining the line
372 : : /// \param[in] q Second point defining the line
373 : : /// \param[in] r Reference point to find the closest point to
374 : : /// \return Closest point on line p-q to point r
375 : : /// \throw std::runtime_error if the two points coincide and hence don't uniquely
376 : : // define a line
377 : : template<typename T>
378 : 0 : inline T closest_line_point_2d(const T & p, const T & q, const T & r)
379 : : {
380 : 0 : const T qp = minus_2d(q, p);
381 : 0 : const float32_t len2 = dot_2d(qp, qp);
382 : 0 : T ret = p;
383 : 0 : if (len2 > std::numeric_limits<float32_t>::epsilon()) {
384 : 0 : const float32_t t = dot_2d(minus_2d(r, p), qp) / len2;
385 : 0 : ret = plus_2d(p, times_2d(qp, t));
386 : : } else {
387 : : throw std::runtime_error(
388 : 0 : "closet_line_point_2d: line ill-defined because given points coincide");
389 : : }
390 : 0 : return ret;
391 : : }
392 : :
393 : : /// \tparam T point type. Must have point adapters defined or have float members x and y
394 : : /// \brief Compute the distance from line segment p-q to point r
395 : : /// \param[in] p First point defining the line segment
396 : : /// \param[in] q Second point defining the line segment
397 : : /// \param[in] r Reference point to find the distance from the line segment to
398 : : /// \return Distance from point r to line segment p-q
399 : : template<typename T>
400 : 0 : inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r)
401 : : {
402 : 0 : const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r);
403 : 0 : return norm_2d(pq_r);
404 : : }
405 : :
406 : : /// \brief Make a 2D unit vector given an angle.
407 : : /// \tparam T Point type. Must have point adapters defined or have float members x and y
408 : : /// \param th Angle in radians
409 : : /// \return Unit vector in the direction of the given angle.
410 : : template<typename T>
411 : : inline T make_unit_vector2d(float th)
412 : : {
413 : : T r;
414 : : point_adapter::xr_(r) = std::cos(th);
415 : : point_adapter::yr_(r) = std::sin(th);
416 : : return r;
417 : : }
418 : :
419 : : /// \brief Compute squared euclidean distance between two points
420 : : /// \tparam OUT return type. Type of the returned distance.
421 : : /// \tparam T1 point type. Must have point adapters defined or have float members x and y
422 : : /// \tparam T2 point type. Must have point adapters defined or have float members x and y
423 : : /// \param a point 1
424 : : /// \param b point 2
425 : : /// \return squared euclidean distance
426 : : template<typename OUT = float32_t, typename T1, typename T2>
427 : : inline OUT squared_distance_2d(const T1 & a, const T2 & b)
428 : : {
429 : : const auto x = static_cast<OUT>(point_adapter::x_(a)) - static_cast<OUT>(point_adapter::x_(b));
430 : : const auto y = static_cast<OUT>(point_adapter::y_(a)) - static_cast<OUT>(point_adapter::y_(b));
431 : : return (x * x) + (y * y);
432 : : }
433 : :
434 : : /// \brief Compute euclidean distance between two points
435 : : /// \tparam OUT return type. Type of the returned distance.
436 : : /// \tparam T1 point type. Must have point adapters defined or have float members x and y
437 : : /// \tparam T2 point type. Must have point adapters defined or have float members x and y
438 : : /// \param a point 1
439 : : /// \param b point 2
440 : : /// \return euclidean distance
441 : : template<typename OUT = float32_t, typename T1, typename T2>
442 : : inline OUT distance_2d(const T1 & a, const T2 & b)
443 : : {
444 : : return std::sqrt(squared_distance_2d<OUT>(a, b));
445 : : }
446 : :
447 : : /// \brief Check the given point's position relative the infinite line passing
448 : : /// from p1 to p2. Logic based on http://geomalgorithms.com/a01-_area.html#isLeft()
449 : : /// \tparam T T point type. Must have point adapters defined or have float members x and y
450 : : /// \param p1 point 1 laying on the infinite line
451 : : /// \param p2 point 2 laying on the infinite line
452 : : /// \param q point to be checked against the line
453 : : /// \return > 0 for point q left of the line through p1 to p2
454 : : /// = 0 for point q on the line through p1 to p2
455 : : /// < 0 for point q right of the line through p1 to p2
456 : : template<typename T>
457 : 0 : inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q)
458 : : {
459 : 0 : return cross_2d(minus_2d(p2, p1), minus_2d(q, p1));
460 : : }
461 : :
462 : : /// Check if all points are ordered in x-y plane (in either clockwise or counter-clockwise
463 : : /// direction): This function does not check for convexity
464 : : /// \tparam IT Iterator type pointing to a point containing float x and float y
465 : : /// \param[in] begin Beginning of point sequence
466 : : /// \param[in] end One past the last of the point sequence
467 : : /// \return Whether or not all point triples p_i, p_{i+1}, p_{i+2} are in a particular order.
468 : : /// Returns true for collinear points as well
469 : : template<typename IT>
470 : 0 : bool all_ordered(const IT begin, const IT end) noexcept
471 : : {
472 : : // Short circuit: a line or point is always CCW or otherwise ill-defined
473 : 0 : if (std::distance(begin, end) <= 2U) {
474 : 0 : return true;
475 : : }
476 : 0 : bool is_first_point_ccw = false;
477 : : // Can't use std::all_of because that works directly on the values
478 : 0 : for (auto line_start = begin; line_start != end; ++line_start) {
479 : 0 : const auto line_end = details::circular_next(begin, end, line_start);
480 : 0 : const auto query_point = details::circular_next(begin, end, line_end);
481 : : // Check if 3 points starting from current point are in counter clockwise direction
482 : 0 : const bool is_ccw = ccw(*line_start, *line_end, *query_point);
483 : 0 : if (is_ccw) {
484 : 0 : if (line_start == begin) {
485 : 0 : is_first_point_ccw = true;
486 : : } else {
487 : 0 : if (!is_first_point_ccw) {
488 : 0 : return false;
489 : : }
490 : : }
491 : 0 : } else if (is_first_point_ccw) {
492 : 0 : return false;
493 : : }
494 : : }
495 : 0 : return true;
496 : : }
497 : :
498 : : /// Compute the area of a convex hull, points are assumed to be ordered (in either CW or CCW)
499 : : /// \tparam IT Iterator type pointing to a point containing float x and float y
500 : : /// \param[in] begin Iterator pointing to the beginning of the polygon points
501 : : /// \param[in] end Iterator pointing to one past the last of the polygon points
502 : : /// \return The area of the polygon, in squared of whatever units your points are in
503 : : template<typename IT>
504 : 0 : auto area_2d(const IT begin, const IT end) noexcept
505 : : {
506 : : using point_adapter::x_;
507 : : using point_adapter::y_;
508 : : using T = decltype(x_(*begin));
509 : 0 : auto area = T{}; // zero initialization
510 : : // use the approach of https://www.mathwords.com/a/area_convex_polygon.htm
511 : 0 : for (auto it = begin; it != end; ++it) {
512 : 0 : const auto next = details::circular_next(begin, end, it);
513 : 0 : area += x_(*it) * y_(*next);
514 : 0 : area -= x_(*next) * y_(*it);
515 : : }
516 : 0 : return std::abs(T{0.5} *area);
517 : : }
518 : :
519 : : /// Compute area of convex hull, throw if points are not ordered (convexity check is not
520 : : /// implemented)
521 : : /// \throw std::domain_error if points are not ordered either CW or CCW
522 : : /// \tparam IT Iterator type pointing to a point containing float x and float y
523 : : /// \param[in] begin Iterator pointing to the beginning of the polygon points
524 : : /// \param[in] end Iterator pointing to one past the last of the polygon points
525 : : /// \return The area of the polygon, in squared of whatever units your points are in
526 : : template<typename IT>
527 : 0 : auto area_checked_2d(const IT begin, const IT end)
528 : : {
529 : 0 : if (!all_ordered(begin, end)) {
530 : 0 : throw std::domain_error{"Cannot compute area: points are not ordered"};
531 : : }
532 : 0 : return area_2d(begin, end);
533 : : }
534 : :
535 : : /// \brief Check if the given point is inside or on the edge of the given polygon
536 : : /// \tparam IteratorType iterator type. The value pointed to by this must have point adapters
537 : : /// defined or have float members x and y
538 : : /// \tparam PointType point type. Must have point adapters defined or have float members x and y
539 : : /// \param start_it iterator pointing to the first vertex of the polygon
540 : : /// \param end_it iterator pointing to the last vertex of the polygon. The vertices should be in
541 : : /// order.
542 : : /// \param p point to be searched
543 : : /// \return True if the point is inside or on the edge of the polygon. False otherwise
544 : : template<typename IteratorType, typename PointType>
545 : 0 : bool is_point_inside_polygon_2d(
546 : : const IteratorType & start_it, const IteratorType & end_it, const PointType & p)
547 : : {
548 : 0 : std::int32_t winding_num = 0;
549 : :
550 : 0 : for (IteratorType it = start_it; it != end_it; ++it) {
551 : 0 : auto next_it = std::next(it);
552 : 0 : if (next_it == end_it) {
553 : 0 : next_it = start_it;
554 : : }
555 : 0 : if (point_adapter::y_(*it) <= point_adapter::y_(p)) {
556 : : // Upward crossing edge
557 : 0 : if (point_adapter::y_(*next_it) >= point_adapter::y_(p)) {
558 : 0 : if (comparison::abs_gt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) {
559 : 0 : ++winding_num;
560 : : } else {
561 : 0 : if (comparison::abs_eq_zero(
562 : 0 : check_point_position_to_line_2d(*it, *next_it, p),
563 : 0 : 1e-3F))
564 : : {
565 : 0 : return true;
566 : : }
567 : : }
568 : : }
569 : : } else {
570 : : // Downward crossing edge
571 : 0 : if (point_adapter::y_(*next_it) <= point_adapter::y_(p)) {
572 : 0 : if (comparison::abs_lt(check_point_position_to_line_2d(*it, *next_it, p), 0.0F, 1e-3F)) {
573 : 0 : --winding_num;
574 : : } else {
575 : 0 : if (comparison::abs_eq_zero(
576 : 0 : check_point_position_to_line_2d(*it, *next_it, p),
577 : 0 : 1e-3F))
578 : : {
579 : 0 : return true;
580 : : }
581 : : }
582 : : }
583 : : }
584 : : }
585 : 0 : return winding_num != 0;
586 : : }
587 : :
588 : :
589 : : } // namespace geometry
590 : : } // namespace common
591 : : } // namespace autoware
592 : :
593 : : #endif // GEOMETRY__COMMON_2D_HPP_
|
