Apollo 10.0
自动驾驶开放平台
polygon2d.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
19#include <algorithm>
20#include <cmath>
21#include <limits>
22#include <utility>
23
24#include "absl/strings/str_cat.h"
25#include "absl/strings/str_join.h"
26
27#include "cyber/common/log.h"
30
31namespace apollo {
32namespace common {
33namespace math {
34
39
40Polygon2d::Polygon2d(std::vector<Vec2d> points, bool check_area)
41 : points_(std::move(points)) {
42 BuildFromPoints(check_area);
43}
44
45double Polygon2d::DistanceTo(const Vec2d &point) const {
46 CHECK_GE(points_.size(), 3U);
47 if (IsPointIn(point)) {
48 return 0.0;
49 }
50 double distance = std::numeric_limits<double>::infinity();
51 for (int i = 0; i < num_points_; ++i) {
52 distance = std::min(distance, line_segments_[i].DistanceTo(point));
53 }
54 return distance;
55}
56
57double Polygon2d::DistanceTo(const Vec2d &point, Vec2d& closest_point) const {
58 CHECK_GE(points_.size(), 3U);
59 if (IsPointIn(point)) {
60 return 0.0;
61 }
62 double distance = std::numeric_limits<double>::infinity();
63 Vec2d closest_point_tmp;
64 for (int i = 0; i < num_points_; ++i) {
65 double dis = line_segments_[i].DistanceTo(point, &closest_point_tmp);
66 if (dis < distance) {
67 distance = dis;
68 closest_point = closest_point_tmp;
69 }
70 }
71 return distance;
72}
73
74double Polygon2d::DistanceSquareTo(const Vec2d &point) const {
75 CHECK_GE(points_.size(), 3U);
76 if (IsPointIn(point)) {
77 return 0.0;
78 }
79 double distance_sqr = std::numeric_limits<double>::infinity();
80 for (int i = 0; i < num_points_; ++i) {
81 distance_sqr =
82 std::min(distance_sqr, line_segments_[i].DistanceSquareTo(point));
83 }
84 return distance_sqr;
85}
86
87double Polygon2d::DistanceTo(const LineSegment2d &line_segment) const {
88 if (line_segment.length() <= kMathEpsilon) {
89 return DistanceTo(line_segment.start());
90 }
91 CHECK_GE(points_.size(), 3U);
92 if (IsPointIn(line_segment.center())) {
93 return 0.0;
94 }
95 if (std::any_of(line_segments_.begin(), line_segments_.end(),
96 [&](const LineSegment2d &poly_seg) {
97 return poly_seg.HasIntersect(line_segment);
98 })) {
99 return 0.0;
100 }
101
102 double distance = std::min(DistanceTo(line_segment.start()),
103 DistanceTo(line_segment.end()));
104 for (int i = 0; i < num_points_; ++i) {
105 distance = std::min(distance, line_segment.DistanceTo(points_[i]));
106 }
107 return distance;
108}
109
110double Polygon2d::DistanceTo(const LineSegment2d &line_segment,
111 Vec2d& polygon_closest_point,
112 Vec2d& segment_closest_point) const {
113 if (line_segment.length() <= kMathEpsilon) {
114 segment_closest_point = line_segment.start();
115 return DistanceTo(line_segment.start(), polygon_closest_point);
116 }
117 CHECK_GE(points_.size(), 3U);
118
119 if (IsPointIn(line_segment.center())) {
120 return 0.0;
121 }
122 if (std::any_of(line_segments_.begin(), line_segments_.end(),
123 [&](const LineSegment2d &poly_seg) {
124 return poly_seg.HasIntersect(line_segment);
125 })) {
126 return 0.0;
127 }
128 double distance = std::numeric_limits<double>::infinity();
129 Vec2d start_closest_point, end_closest_point;
130 double distance_start = DistanceTo(line_segment.start(), start_closest_point);
131 double distance_end = DistanceTo(line_segment.end(), end_closest_point);
132 if (distance_start <= distance_end) {
133 segment_closest_point = line_segment.start();
134 polygon_closest_point = start_closest_point;
135 distance = distance_start;
136 } else {
137 segment_closest_point = line_segment.end();
138 polygon_closest_point = end_closest_point;
139 distance = distance_end;
140 }
141 Vec2d closest_point_tmp;
142 for (int i = 0; i < num_points_; ++i) {
143 double dis = line_segment.DistanceTo(points_[i], &closest_point_tmp);
144 if (dis < distance) {
145 distance = dis;
146 segment_closest_point = closest_point_tmp;
147 polygon_closest_point = points_[i];
148 }
149 }
150 return distance;
151}
152
153double Polygon2d::DistanceTo(const Box2d &box) const {
154 CHECK_GE(points_.size(), 3U);
155 return DistanceTo(Polygon2d(box));
156}
157
158double Polygon2d::DistanceTo(const Polygon2d &polygon) const {
159 CHECK_GE(points_.size(), 3U);
160 CHECK_GE(polygon.num_points(), 3);
161
162 if (IsPointIn(polygon.points()[0])) {
163 return 0.0;
164 }
165 if (polygon.IsPointIn(points_[0])) {
166 return 0.0;
167 }
168 double distance = std::numeric_limits<double>::infinity();
169 for (int i = 0; i < num_points_; ++i) {
170 distance = std::min(distance, polygon.DistanceTo(line_segments_[i]));
171 }
172 return distance;
173}
174
175double Polygon2d::DistanceTo(const Polygon2d& polygon,
176 Vec2d& self_closest_point,
177 Vec2d& other_closest_point) const {
178 CHECK_GE(points_.size(), 3U);
179 CHECK_GE(polygon.num_points(), 3);
180 double distance = std::numeric_limits<double>::infinity();
181 Vec2d segment_closest_point_tmp, polygon_closest_point_tmp;
182 for (int i = 0; i < num_points_; ++i) {
183 double dis = polygon.DistanceTo(line_segments_[i],
184 polygon_closest_point_tmp, segment_closest_point_tmp);
185 if (dis < distance) {
186 distance = dis;
187 self_closest_point = segment_closest_point_tmp;
188 other_closest_point = polygon_closest_point_tmp;
189 }
190 }
191 return distance;
192}
193
194double Polygon2d::DistanceToBoundary(const Vec2d &point) const {
195 double distance = std::numeric_limits<double>::infinity();
196 for (int i = 0; i < num_points_; ++i) {
197 distance = std::min(distance, line_segments_[i].DistanceTo(point));
198 }
199 return distance;
200}
201
202bool Polygon2d::IsPointOnBoundary(const Vec2d &point) const {
203 CHECK_GE(points_.size(), 3U);
204 return std::any_of(
205 line_segments_.begin(), line_segments_.end(),
206 [&](const LineSegment2d &poly_seg) { return poly_seg.IsPointIn(point); });
207}
208
209bool Polygon2d::IsPointIn(const Vec2d &point) const {
210 CHECK_GE(points_.size(), 3U);
211 if (IsPointOnBoundary(point)) {
212 return true;
213 }
214 int j = num_points_ - 1;
215 int c = 0;
216 for (int i = 0; i < num_points_; ++i) {
217 if ((points_[i].y() > point.y()) != (points_[j].y() > point.y())) {
218 const double side = CrossProd(point, points_[i], points_[j]);
219 if (points_[i].y() < points_[j].y() ? side > 0.0 : side < 0.0) {
220 ++c;
221 }
222 }
223 j = i;
224 }
225 return c & 1;
226}
227
228bool Polygon2d::HasOverlap(const Polygon2d &polygon) const {
229 CHECK_GE(points_.size(), 3U);
230 CHECK_GE(polygon.num_points(), 3);
231 if (polygon.max_x() < min_x() || polygon.min_x() > max_x() ||
232 polygon.max_y() < min_y() || polygon.min_y() > max_y()) {
233 return false;
234 }
235
236 if (IsPointIn(polygon.points()[0])) {
237 return true;
238 }
239
240 if (polygon.IsPointIn(points_[0])) {
241 return true;
242 }
243
244 for (int i = 0; i < polygon.num_points(); ++i) {
245 if (HasOverlap(polygon.line_segments()[i])) {
246 return true;
247 }
248 }
249 return false;
250}
251
252bool Polygon2d::Contains(const LineSegment2d &line_segment) const {
253 if (line_segment.length() <= kMathEpsilon) {
254 return IsPointIn(line_segment.start());
255 }
256 CHECK_GE(points_.size(), 3U);
257 if (!IsPointIn(line_segment.start())) {
258 return false;
259 }
260 if (!IsPointIn(line_segment.end())) {
261 return false;
262 }
263 if (!is_convex_) {
264 std::vector<LineSegment2d> overlaps = GetAllOverlaps(line_segment);
265 double total_length = 0;
266 for (const auto &overlap_seg : overlaps) {
267 total_length += overlap_seg.length();
268 }
269 return total_length >= line_segment.length() - kMathEpsilon;
270 }
271 return true;
272}
273
274bool Polygon2d::Contains(const Polygon2d &polygon) const {
275 CHECK_GE(points_.size(), 3U);
276 if (area_ < polygon.area() - kMathEpsilon) {
277 return false;
278 }
279 if (!IsPointIn(polygon.points()[0])) {
280 return false;
281 }
282 const auto &line_segments = polygon.line_segments();
283 return std::all_of(line_segments.begin(), line_segments.end(),
284 [&](const LineSegment2d &line_segment) {
285 return Contains(line_segment);
286 });
287}
288
289int Polygon2d::Next(int at) const { return at >= num_points_ - 1 ? 0 : at + 1; }
290
291int Polygon2d::Prev(int at) const { return at == 0 ? num_points_ - 1 : at - 1; }
292
293void Polygon2d::BuildFromPoints(bool check_area) {
294 num_points_ = static_cast<int>(points_.size());
295 CHECK_GE(num_points_, 3);
296
297 // Make sure the points are in ccw order.
298 area_ = 0.0;
299 for (int i = 1; i < num_points_; ++i) {
300 area_ += CrossProd(points_[0], points_[i - 1], points_[i]);
301 }
302 if (area_ < 0) {
303 area_ = -area_;
304 std::reverse(points_.begin(), points_.end());
305 }
306 area_ /= 2.0;
307 if (check_area) {
308 CHECK_GT(area_, kMathEpsilon);
309 }
310
311 // Construct line_segments.
313 for (int i = 0; i < num_points_; ++i) {
314 line_segments_.emplace_back(points_[i], points_[Next(i)]);
315 }
316
317 // Check convexity.
318 is_convex_ = true;
319 for (int i = 0; i < num_points_; ++i) {
320 if (CrossProd(points_[Prev(i)], points_[i], points_[Next(i)]) <=
321 -kMathEpsilon) {
322 is_convex_ = false;
323 break;
324 }
325 }
326
327 // Compute aabox.
328 min_x_ = points_[0].x();
329 max_x_ = points_[0].x();
330 min_y_ = points_[0].y();
331 max_y_ = points_[0].y();
332 for (const auto &point : points_) {
333 min_x_ = std::min(min_x_, point.x());
334 max_x_ = std::max(max_x_, point.x());
335 min_y_ = std::min(min_y_, point.y());
336 max_y_ = std::max(max_y_, point.y());
337 }
338}
339
340bool Polygon2d::ComputeConvexHull(const std::vector<Vec2d> &points,
341 Polygon2d *const polygon, bool check_area) {
342 CHECK_NOTNULL(polygon);
343 const int n = static_cast<int>(points.size());
344 if (n < 3) {
345 return false;
346 }
347 std::vector<int> sorted_indices(n);
348 for (int i = 0; i < n; ++i) {
349 sorted_indices[i] = i;
350 }
351 std::sort(sorted_indices.begin(), sorted_indices.end(),
352 [&](const int idx1, const int idx2) {
353 const Vec2d &pt1 = points[idx1];
354 const Vec2d &pt2 = points[idx2];
355 const double dx = pt1.x() - pt2.x();
356 if (std::abs(dx) > kMathEpsilon) {
357 return dx < 0.0;
358 }
359 return pt1.y() < pt2.y();
360 });
361 int count = 0;
362 std::vector<int> results;
363 results.reserve(n);
364 int last_count = 1;
365 for (int i = 0; i < n + n; ++i) {
366 if (i == n) {
367 last_count = count;
368 }
369 const int idx = sorted_indices[(i < n) ? i : (n + n - 1 - i)];
370 const Vec2d &pt = points[idx];
371 while (count > last_count &&
372 CrossProd(points[results[count - 2]], points[results[count - 1]],
373 pt) <= kMathEpsilon) {
374 results.pop_back();
375 --count;
376 }
377 results.push_back(idx);
378 ++count;
379 }
380 --count;
381 if (count < 3) {
382 return false;
383 }
384 std::vector<Vec2d> result_points;
385 result_points.reserve(count);
386 for (int i = 0; i < count; ++i) {
387 result_points.push_back(points[results[i]]);
388 }
389 *polygon = Polygon2d(result_points, check_area);
390 return true;
391}
392
393bool Polygon2d::ClipConvexHull(const LineSegment2d &line_segment,
394 std::vector<Vec2d> *const points) {
395 if (line_segment.length() <= kMathEpsilon) {
396 return true;
397 }
398 CHECK_NOTNULL(points);
399 const size_t n = points->size();
400 if (n < 3) {
401 return false;
402 }
403 std::vector<double> prod(n);
404 std::vector<int> side(n);
405 for (size_t i = 0; i < n; ++i) {
406 prod[i] = CrossProd(line_segment.start(), line_segment.end(), (*points)[i]);
407 if (std::abs(prod[i]) <= kMathEpsilon) {
408 side[i] = 0;
409 } else {
410 side[i] = ((prod[i] < 0) ? -1 : 1);
411 }
412 }
413
414 std::vector<Vec2d> new_points;
415 for (size_t i = 0; i < n; ++i) {
416 if (side[i] >= 0) {
417 new_points.push_back((*points)[i]);
418 }
419 const size_t j = ((i == n - 1) ? 0 : (i + 1));
420 if (side[i] * side[j] < 0) {
421 const double ratio = prod[j] / (prod[j] - prod[i]);
422 new_points.emplace_back(
423 (*points)[i].x() * ratio + (*points)[j].x() * (1.0 - ratio),
424 (*points)[i].y() * ratio + (*points)[j].y() * (1.0 - ratio));
425 }
426 }
427
428 points->swap(new_points);
429 return points->size() >= 3U;
430}
431
432bool Polygon2d::ComputeOverlap(const Polygon2d &other_polygon,
433 Polygon2d *const overlap_polygon) const {
434 CHECK_GE(points_.size(), 3U);
435 CHECK_NOTNULL(overlap_polygon);
436 ACHECK(is_convex_ && other_polygon.is_convex());
437 std::vector<Vec2d> points = other_polygon.points();
438 for (int i = 0; i < num_points_; ++i) {
439 if (!ClipConvexHull(line_segments_[i], &points)) {
440 return false;
441 }
442 }
443 return ComputeConvexHull(points, overlap_polygon, false);
444}
445
446double Polygon2d::ComputeIoU(const Polygon2d &other_polygon) const {
447 Polygon2d overlap_polygon;
448 if (!ComputeOverlap(other_polygon, &overlap_polygon)) {
449 return 0.0;
450 }
451 double intersection_area = overlap_polygon.area();
452 double union_area = area_ + other_polygon.area() - overlap_polygon.area();
453 return intersection_area / union_area;
454}
455
456bool Polygon2d::HasOverlap(const LineSegment2d &line_segment) const {
457 CHECK_GE(points_.size(), 3U);
458 if ((line_segment.start().x() < min_x_ && line_segment.end().x() < min_x_) ||
459 (line_segment.start().x() > max_x_ && line_segment.end().x() > max_x_) ||
460 (line_segment.start().y() < min_y_ && line_segment.end().y() < min_y_) ||
461 (line_segment.start().y() > max_y_ && line_segment.end().y() > max_y_)) {
462 return false;
463 }
464
465 if (std::any_of(line_segments_.begin(), line_segments_.end(),
466 [&](const LineSegment2d &poly_seg) {
467 return poly_seg.HasIntersect(line_segment);
468 })) {
469 return true;
470 }
471 return false;
472}
473
474bool Polygon2d::GetOverlap(const LineSegment2d &line_segment,
475 Vec2d *const first, Vec2d *const last) const {
476 CHECK_GE(points_.size(), 3U);
477 CHECK_NOTNULL(first);
478 CHECK_NOTNULL(last);
479
480 if (line_segment.length() <= kMathEpsilon) {
481 if (!IsPointIn(line_segment.start())) {
482 return false;
483 }
484 *first = line_segment.start();
485 *last = line_segment.start();
486 return true;
487 }
488
489 double min_proj = line_segment.length();
490 double max_proj = 0;
491 if (IsPointIn(line_segment.start())) {
492 *first = line_segment.start();
493 min_proj = 0.0;
494 }
495 if (IsPointIn(line_segment.end())) {
496 *last = line_segment.end();
497 max_proj = line_segment.length();
498 }
499 for (const auto &poly_seg : line_segments_) {
500 Vec2d pt;
501 if (poly_seg.GetIntersect(line_segment, &pt)) {
502 const double proj = line_segment.ProjectOntoUnit(pt);
503 if (proj < min_proj) {
504 min_proj = proj;
505 *first = pt;
506 }
507 if (proj > max_proj) {
508 max_proj = proj;
509 *last = pt;
510 }
511 }
512 }
513 return min_proj <= max_proj + kMathEpsilon;
514}
515
516void Polygon2d::GetAllVertices(std::vector<Vec2d> *const vertices) const {
517 if (vertices == nullptr) {
518 return;
519 }
520 *vertices = points_;
521}
522
523std::vector<Vec2d> Polygon2d::GetAllVertices() const { return points_; }
524
525std::vector<LineSegment2d> Polygon2d::GetAllOverlaps(
526 const LineSegment2d &line_segment) const {
527 CHECK_GE(points_.size(), 3U);
528
529 if (line_segment.length() <= kMathEpsilon) {
530 std::vector<LineSegment2d> overlaps;
531 if (IsPointIn(line_segment.start())) {
532 overlaps.push_back(line_segment);
533 }
534 return overlaps;
535 }
536 std::vector<double> projections;
537 if (IsPointIn(line_segment.start())) {
538 projections.push_back(0.0);
539 }
540 if (IsPointIn(line_segment.end())) {
541 projections.push_back(line_segment.length());
542 }
543 for (const auto &poly_seg : line_segments_) {
544 Vec2d pt;
545 if (poly_seg.GetIntersect(line_segment, &pt)) {
546 projections.push_back(line_segment.ProjectOntoUnit(pt));
547 }
548 }
549 std::sort(projections.begin(), projections.end());
550 std::vector<std::pair<double, double>> overlaps;
551 for (size_t i = 0; i + 1 < projections.size(); ++i) {
552 const double start_proj = projections[i];
553 const double end_proj = projections[i + 1];
554 if (end_proj - start_proj <= kMathEpsilon) {
555 continue;
556 }
557 const Vec2d reference_point =
558 line_segment.start() +
559 (start_proj + end_proj) / 2.0 * line_segment.unit_direction();
560 if (!IsPointIn(reference_point)) {
561 continue;
562 }
563 if (overlaps.empty() ||
564 start_proj > overlaps.back().second + kMathEpsilon) {
565 overlaps.emplace_back(start_proj, end_proj);
566 } else {
567 overlaps.back().second = end_proj;
568 }
569 }
570 std::vector<LineSegment2d> overlap_line_segments;
571 for (const auto &overlap : overlaps) {
572 overlap_line_segments.emplace_back(
573 line_segment.start() + overlap.first * line_segment.unit_direction(),
574 line_segment.start() + overlap.second * line_segment.unit_direction());
575 }
576 return overlap_line_segments;
577}
578
579void Polygon2d::ExtremePoints(const double heading, Vec2d *const first,
580 Vec2d *const last) const {
581 CHECK_GE(points_.size(), 3U);
582 CHECK_NOTNULL(first);
583 CHECK_NOTNULL(last);
584
585 const Vec2d direction_vec = Vec2d::CreateUnitVec2d(heading);
586 double min_proj = std::numeric_limits<double>::infinity();
587 double max_proj = -std::numeric_limits<double>::infinity();
588 for (const auto &pt : points_) {
589 const double proj = pt.InnerProd(direction_vec);
590 if (proj < min_proj) {
591 min_proj = proj;
592 *first = pt;
593 }
594 if (proj > max_proj) {
595 max_proj = proj;
596 *last = pt;
597 }
598 }
599}
600
601AABox2d Polygon2d::AABoundingBox() const {
602 return AABox2d({min_x_, min_y_}, {max_x_, max_y_});
603}
604
605Box2d Polygon2d::BoundingBoxWithHeading(const double heading) const {
606 CHECK_GE(points_.size(), 3U);
607 const Vec2d direction_vec = Vec2d::CreateUnitVec2d(heading);
608 Vec2d px1;
609 Vec2d px2;
610 Vec2d py1;
611 Vec2d py2;
612 ExtremePoints(heading, &px1, &px2);
613 ExtremePoints(heading - M_PI_2, &py1, &py2);
614 const double x1 = px1.InnerProd(direction_vec);
615 const double x2 = px2.InnerProd(direction_vec);
616 const double y1 = py1.CrossProd(direction_vec);
617 const double y2 = py2.CrossProd(direction_vec);
618 return Box2d(
619 (x1 + x2) / 2.0 * direction_vec +
620 (y1 + y2) / 2.0 * Vec2d(direction_vec.y(), -direction_vec.x()),
621 heading, x2 - x1, y2 - y1);
622}
623
624Box2d Polygon2d::MinAreaBoundingBox() const {
625 CHECK_GE(points_.size(), 3U);
626 if (!is_convex_) {
627 Polygon2d convex_polygon;
628 ComputeConvexHull(points_, &convex_polygon);
629 ACHECK(convex_polygon.is_convex());
630 return convex_polygon.MinAreaBoundingBox();
631 }
632 double min_area = std::numeric_limits<double>::infinity();
633 double min_area_at_heading = 0.0;
634 int left_most = 0;
635 int right_most = 0;
636 int top_most = 0;
637 for (int i = 0; i < num_points_; ++i) {
638 const auto &line_segment = line_segments_[i];
639 double proj = 0.0;
640 double min_proj = line_segment.ProjectOntoUnit(points_[left_most]);
641 while ((proj = line_segment.ProjectOntoUnit(points_[Prev(left_most)])) <
642 min_proj) {
643 min_proj = proj;
644 left_most = Prev(left_most);
645 }
646 while ((proj = line_segment.ProjectOntoUnit(points_[Next(left_most)])) <
647 min_proj) {
648 min_proj = proj;
649 left_most = Next(left_most);
650 }
651 double max_proj = line_segment.ProjectOntoUnit(points_[right_most]);
652 while ((proj = line_segment.ProjectOntoUnit(points_[Prev(right_most)])) >
653 max_proj) {
654 max_proj = proj;
655 right_most = Prev(right_most);
656 }
657 while ((proj = line_segment.ProjectOntoUnit(points_[Next(right_most)])) >
658 max_proj) {
659 max_proj = proj;
660 right_most = Next(right_most);
661 }
662 double prod = 0.0;
663 double max_prod = line_segment.ProductOntoUnit(points_[top_most]);
664 while ((prod = line_segment.ProductOntoUnit(points_[Prev(top_most)])) >
665 max_prod) {
666 max_prod = prod;
667 top_most = Prev(top_most);
668 }
669 while ((prod = line_segment.ProductOntoUnit(points_[Next(top_most)])) >
670 max_prod) {
671 max_prod = prod;
672 top_most = Next(top_most);
673 }
674 const double area = max_prod * (max_proj - min_proj);
675 if (area < min_area) {
676 min_area = area;
677 min_area_at_heading = line_segment.heading();
678 }
679 }
680 return BoundingBoxWithHeading(min_area_at_heading);
681}
682
683Polygon2d Polygon2d::ExpandByDistance(const double distance) const {
684 if (!is_convex_) {
685 Polygon2d convex_polygon;
686 ComputeConvexHull(points_, &convex_polygon);
687 ACHECK(convex_polygon.is_convex());
688 return convex_polygon.ExpandByDistance(distance);
689 }
690 const double kMinAngle = 0.1;
691 std::vector<Vec2d> points;
692 for (int i = 0; i < num_points_; ++i) {
693 const double start_angle = line_segments_[Prev(i)].heading() - M_PI_2;
694 const double end_angle = line_segments_[i].heading() - M_PI_2;
695 const double diff = WrapAngle(end_angle - start_angle);
696 if (diff <= kMathEpsilon) {
697 points.push_back(points_[i] +
698 Vec2d::CreateUnitVec2d(start_angle) * distance);
699 } else {
700 const int count = static_cast<int>(diff / kMinAngle) + 1;
701 for (int k = 0; k <= count; ++k) {
702 const double angle = start_angle + diff * static_cast<double>(k) /
703 static_cast<double>(count);
704 points.push_back(points_[i] + Vec2d::CreateUnitVec2d(angle) * distance);
705 }
706 }
707 }
708 Polygon2d new_polygon;
709 ACHECK(ComputeConvexHull(points, &new_polygon));
710 return new_polygon;
711}
712
713Polygon2d Polygon2d::PolygonExpandByDistance(const double distance) const {
714 if (!is_convex_) {
715 Polygon2d convex_polygon;
716 ComputeConvexHull(points_, &convex_polygon);
717 ACHECK(convex_polygon.is_convex());
718 return convex_polygon.PolygonExpandByDistance(distance);
719 }
720 std::vector<Vec2d> points;
721 for (int i = 0; i < num_points_; ++i) {
722 double v1x = points_[Prev(i)].x() - points_[i].x();
723 double v1y = points_[Prev(i)].y() - points_[i].y();
724 double n1 = std::sqrt(v1x * v1x + v1y * v1y);
725 v1x /= n1;
726 v1y /= n1;
727
728 double v2x = points_[Next(i)].x() - points_[i].x();
729 double v2y = points_[Next(i)].y() - points_[i].y();
730 double n2 = std::sqrt(v2x * v2x + v2y * v2y);
731 v2x /= n2;
732 v2x /= n2;
733
734 double l = distance / sqrt((1 - (v1x * v2x + v1y * v2y)) / 2);
735
736 double vx = v1x + v2x;
737 double vy = v1y + v2y;
738 double n = l / sqrt(vx * vx + vy * vy);
739
740 vx *= n;
741 vy *= n;
742
743 Vec2d point(vx + points_[i].x(), vy + points_[i].y());
744 points.push_back(point);
745 }
746 Polygon2d new_polygon;
747 ACHECK(ComputeConvexHull(points, &new_polygon));
748 return new_polygon;
749}
750
751void Polygon2d::CalculateVertices(const Vec2d &shift_vec) {
752 for (size_t i = 0; i < num_points_; ++i) {
753 points_[i] += shift_vec;
754 }
755 for (auto &point : points_) {
756 max_x_ = std::fmax(point.x(), max_x_);
757 min_x_ = std::fmin(point.x(), min_x_);
758 max_y_ = std::fmax(point.y(), max_y_);
759 min_y_ = std::fmin(point.y(), min_y_);
760 }
761}
762
763LineSegment2d Polygon2d::MinLineSegment() const {
764 double min_length_of_line_segment = 10e3;
765 LineSegment2d line_segment;
766 for (auto &line_segment_ : line_segments_) {
767 if (line_segment_.length() < min_length_of_line_segment) {
768 line_segment = line_segment_;
769 }
770 }
771 return line_segment;
772}
773
774std::string Polygon2d::DebugString() const {
775 return absl::StrCat("polygon2d ( num_points = ", num_points_, " points = (",
776 absl::StrJoin(points_, " ", util::DebugStringFormatter()),
777 " ) ", is_convex_ ? "convex" : "non-convex",
778 " area = ", area_, " )");
779}
780
781} // namespace math
782} // namespace common
783} // namespace apollo
Implements a class of (undirected) axes-aligned bounding boxes in 2-D.
Definition aabox2d.h:42
Rectangular (undirected) bounding box in 2-D.
Definition box2d.h:52
void GetAllCorners(std::vector< Vec2d > *const corners) const
Getter of the corners of the box
Definition box2d.cc:140
const Vec2d & end() const
Get the end point.
const Vec2d & unit_direction() const
Get the unit direction from the start point to the end point.
double ProjectOntoUnit(const Vec2d &point) const
Compute the projection of a vector onto the line segment.
double length() const
Get the length of the line segment.
double DistanceTo(const Vec2d &point) const
Compute the shortest distance from a point on the line segment to a point in 2-D.
const Vec2d & start() const
Get the start point.
Vec2d center() const
Get the center of the line segment.
The class of polygon in 2-D.
Definition polygon2d.h:42
std::vector< LineSegment2d > GetAllOverlaps(const LineSegment2d &line_segment) const
Get all overlapped line segments of a line segment and this polygon.
Definition polygon2d.cc:525
bool HasOverlap(const LineSegment2d &line_segment) const
Check if a line segment has overlap with this polygon.
Definition polygon2d.cc:456
std::vector< Vec2d > points_
Definition polygon2d.h:379
bool IsPointIn(const Vec2d &point) const
Check if a point is within the polygon.
Definition polygon2d.cc:209
static bool ComputeConvexHull(const std::vector< Vec2d > &points, Polygon2d *const polygon, bool check_area=true)
Compute the convex hull of a group of points.
Definition polygon2d.cc:340
bool Contains(const LineSegment2d &line_segment) const
Check if the polygon contains a line segment.
Definition polygon2d.cc:252
Polygon2d()=default
Empty constructor.
Polygon2d ExpandByDistance(const double distance) const
Expand this polygon by a distance.
Definition polygon2d.cc:683
Box2d MinAreaBoundingBox() const
Get the bounding box with the minimal area.
Definition polygon2d.cc:624
double DistanceTo(const Vec2d &point) const
Compute the distance from a point to the polygon.
Definition polygon2d.cc:45
const std::vector< LineSegment2d > & line_segments() const
Get the edges of the polygon.
Definition polygon2d.h:71
const std::vector< Vec2d > & points() const
Get the vertices of the polygon.
Definition polygon2d.h:65
double DistanceSquareTo(const Vec2d &point) const
Compute the square of distance from a point to the polygon.
Definition polygon2d.cc:74
void BuildFromPoints(bool check_area=true)
Definition polygon2d.cc:293
std::vector< LineSegment2d > line_segments_
Definition polygon2d.h:381
bool IsPointOnBoundary(const Vec2d &point) const
Check if a point is on the boundary of the polygon.
Definition polygon2d.cc:202
int num_points() const
Get the number of vertices of the polygon.
Definition polygon2d.h:79
Polygon2d PolygonExpandByDistance(const double distance) const
Definition polygon2d.cc:713
bool is_convex() const
Check if the polygon is convex.
Definition polygon2d.h:85
double area() const
Get the area of the polygon.
Definition polygon2d.h:91
double DistanceToBoundary(const Vec2d &point) const
Compute the distance from a point to the boundary of the polygon.
Definition polygon2d.cc:194
Implements a class of 2-dimensional vectors.
Definition vec2d.h:42
double InnerProd(const Vec2d &other) const
Returns the inner product between these two Vec2d.
Definition vec2d.cc:61
double y() const
Getter for y component
Definition vec2d.h:57
double DistanceTo(const Vec2d &other) const
Returns the distance to the given vector
Definition vec2d.cc:47
double x() const
Getter for x component
Definition vec2d.h:54
double CrossProd(const Vec2d &other) const
Returns the "cross" product between these two Vec2d (non-standard).
Definition vec2d.cc:57
#define ACHECK(cond)
Definition log.h:80
Math-related util functions.
constexpr double kMathEpsilon
Definition vec2d.h:35
double CrossProd(const Vec2d &start_point, const Vec2d &end_point_1, const Vec2d &end_point_2)
Cross product between two 2-D vectors from the common start point, and end at two other points.
Definition math_utils.cc:28
double WrapAngle(const double angle)
Wrap angle to [0, 2 * PI).
Definition math_utils.cc:48
class register implement
Definition arena_queue.h:37
Definition future.h:29
Define the Polygon2d class.
Some string util functions.