Apollo 10.0
自动驾驶开放平台
apollo::common::math::Polygon2d类 参考

The class of polygon in 2-D. 更多...

#include <polygon2d.h>

类 apollo::common::math::Polygon2d 继承关系图:
apollo::common::math::Polygon2d 的协作图:

Public 成员函数

 Polygon2d ()=default
 Empty constructor.
 
 Polygon2d (const Box2d &box)
 Constructor which takes a box.
 
 Polygon2d (std::vector< Vec2d > points, bool check_area=true)
 Constructor which takes a vector of points as its vertices.
 
const std::vector< Vec2d > & points () const
 Get the vertices of the polygon.
 
const std::vector< LineSegment2d > & line_segments () const
 Get the edges of the polygon.
 
int num_points () const
 Get the number of vertices of the polygon.
 
bool is_convex () const
 Check if the polygon is convex.
 
double area () const
 Get the area of the polygon.
 
double DistanceToBoundary (const Vec2d &point) const
 Compute the distance from a point to the boundary of the polygon.
 
double DistanceTo (const Vec2d &point) const
 Compute the distance from a point to the polygon.
 
double DistanceTo (const Vec2d &point, Vec2d &closest_point) const
 Compute the distance from a point to the polygon.
 
double DistanceTo (const LineSegment2d &line_segment) const
 Compute the distance from a line segment to the polygon.
 
double DistanceTo (const LineSegment2d &line_segment, Vec2d &polygon_closest_point, Vec2d &segment_closest_point) const
 Compute the distance from a line segment to the polygon.
 
double DistanceTo (const Box2d &box) const
 Compute the distance from a box to the polygon.
 
double DistanceTo (const Polygon2d &polygon) const
 Compute the distance from another polygon to the polygon.
 
double DistanceTo (const Polygon2d &polygon, Vec2d &self_closest_point, Vec2d &other_closest_point) const
 Compute the distance from another polygon to the polygon.
 
double DistanceSquareTo (const Vec2d &point) const
 Compute the square of distance from a point to the polygon.
 
bool IsPointIn (const Vec2d &point) const
 Check if a point is within the polygon.
 
bool IsPointOnBoundary (const Vec2d &point) const
 Check if a point is on the boundary of the polygon.
 
bool Contains (const LineSegment2d &line_segment) const
 Check if the polygon contains a line segment.
 
bool Contains (const Polygon2d &polygon) const
 Check if the polygon contains another polygon.
 
bool HasOverlap (const LineSegment2d &line_segment) const
 Check if a line segment has overlap with this polygon.
 
bool GetOverlap (const LineSegment2d &line_segment, Vec2d *const first, Vec2d *const last) const
 Get the overlap of a line segment and this polygon.
 
void GetAllVertices (std::vector< Vec2d > *const vertices) const
 Get all vertices of the polygon
 
std::vector< Vec2dGetAllVertices () const
 Get all vertices of the polygon
 
std::vector< LineSegment2dGetAllOverlaps (const LineSegment2d &line_segment) const
 Get all overlapped line segments of a line segment and this polygon.
 
bool HasOverlap (const Polygon2d &polygon) const
 Check if this polygon has overlap with another polygon.
 
bool ComputeOverlap (const Polygon2d &other_polygon, Polygon2d *const overlap_polygon) const
 Compute the overlap of this polygon and the other polygon if any.
 
double ComputeIoU (const Polygon2d &other_polygon) const
 Compute intersection over union ratio of this polygon and the other polygon.
 
AABox2d AABoundingBox () const
 Get the axis-aligned bound box of the polygon.
 
Box2d BoundingBoxWithHeading (const double heading) const
 Get the bound box according to a heading.
 
Box2d MinAreaBoundingBox () const
 Get the bounding box with the minimal area.
 
void ExtremePoints (const double heading, Vec2d *const first, Vec2d *const last) const
 Get the extreme points along a heading direction.
 
Polygon2d ExpandByDistance (const double distance) const
 Expand this polygon by a distance.
 
Polygon2d PolygonExpandByDistance (const double distance) const
 
void CalculateVertices (const Vec2d &shift_vec)
 
std::string DebugString () const
 Get a string containing essential information about the polygon for debugging purpose.
 
double min_x () const
 
double max_x () const
 
double min_y () const
 
double max_y () const
 
LineSegment2d MinLineSegment () const
 

静态 Public 成员函数

static bool ComputeConvexHull (const std::vector< Vec2d > &points, Polygon2d *const polygon, bool check_area=true)
 Compute the convex hull of a group of points.
 

Protected 成员函数

void BuildFromPoints (bool check_area=true)
 
int Next (int at) const
 
int Prev (int at) const
 

静态 Protected 成员函数

static bool ClipConvexHull (const LineSegment2d &line_segment, std::vector< Vec2d > *const points)
 

Protected 属性

std::vector< Vec2dpoints_
 
int num_points_ = 0
 
std::vector< LineSegment2dline_segments_
 
bool is_convex_ = false
 
double area_ = 0.0
 
double min_x_ = 0.0
 
double max_x_ = 0.0
 
double min_y_ = 0.0
 
double max_y_ = 0.0
 

详细描述

The class of polygon in 2-D.

在文件 polygon2d.h42 行定义.

构造及析构函数说明

◆ Polygon2d() [1/3]

apollo::common::math::Polygon2d::Polygon2d ( )
default

Empty constructor.

◆ Polygon2d() [2/3]

apollo::common::math::Polygon2d::Polygon2d ( const Box2d box)
explicit

Constructor which takes a box.

参数
boxThe box to construct the polygon.

在文件 polygon2d.cc35 行定义.

35 {
36 box.GetAllCorners(&points_);
38}
std::vector< Vec2d > points_
Definition polygon2d.h:379
void BuildFromPoints(bool check_area=true)
Definition polygon2d.cc:293

◆ Polygon2d() [3/3]

apollo::common::math::Polygon2d::Polygon2d ( std::vector< Vec2d points,
bool  check_area = true 
)
explicit

Constructor which takes a vector of points as its vertices.

参数
pointsThe points to construct the polygon.

在文件 polygon2d.cc40 行定义.

41 : points_(std::move(points)) {
42 BuildFromPoints(check_area);
43}
const std::vector< Vec2d > & points() const
Get the vertices of the polygon.
Definition polygon2d.h:65

成员函数说明

◆ AABoundingBox()

AABox2d apollo::common::math::Polygon2d::AABoundingBox ( ) const

Get the axis-aligned bound box of the polygon.

返回
The axis-aligned bound box of the polygon.

在文件 polygon2d.cc601 行定义.

◆ area()

double apollo::common::math::Polygon2d::area ( ) const
inline

Get the area of the polygon.

返回
The area of the polygon.

在文件 polygon2d.h91 行定义.

91{ return area_; }

◆ BoundingBoxWithHeading()

Box2d apollo::common::math::Polygon2d::BoundingBoxWithHeading ( const double  heading) const

Get the bound box according to a heading.

参数
headingThe specified heading of the bounding box.
返回
The bound box according to the specified heading.

在文件 polygon2d.cc605 行定义.

605 {
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}
void ExtremePoints(const double heading, Vec2d *const first, Vec2d *const last) const
Get the extreme points along a heading direction.
Definition polygon2d.cc:579
static Vec2d CreateUnitVec2d(const double angle)
Creates a unit-vector with a given angle to the positive x semi-axis
Definition vec2d.cc:29

◆ BuildFromPoints()

void apollo::common::math::Polygon2d::BuildFromPoints ( bool  check_area = true)
protected

在文件 polygon2d.cc293 行定义.

293 {
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}
std::vector< LineSegment2d > line_segments_
Definition polygon2d.h:381
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

◆ CalculateVertices()

void apollo::common::math::Polygon2d::CalculateVertices ( const Vec2d shift_vec)

在文件 polygon2d.cc751 行定义.

751 {
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}

◆ ClipConvexHull()

bool apollo::common::math::Polygon2d::ClipConvexHull ( const LineSegment2d line_segment,
std::vector< Vec2d > *const  points 
)
staticprotected

在文件 polygon2d.cc393 行定义.

394 {
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}

◆ ComputeConvexHull()

bool apollo::common::math::Polygon2d::ComputeConvexHull ( const std::vector< Vec2d > &  points,
Polygon2d *const  polygon,
bool  check_area = true 
)
static

Compute the convex hull of a group of points.

参数
pointsThe target points. To compute the convex hull of them.
polygonThe convex hull of the points.
返回
If successfully compute the convex hull.

在文件 polygon2d.cc340 行定义.

341 {
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}
Polygon2d()=default
Empty constructor.

◆ ComputeIoU()

double apollo::common::math::Polygon2d::ComputeIoU ( const Polygon2d other_polygon) const

Compute intersection over union ratio of this polygon and the other polygon.

Note: this function only works for computing overlap between two convex polygons.

参数
other_polygonThe target polygon. To compute its overlap with this polygon.
返回
A value between 0.0 and 1.0, meaning no intersection to fully overlaping

在文件 polygon2d.cc446 行定义.

446 {
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}
bool ComputeOverlap(const Polygon2d &other_polygon, Polygon2d *const overlap_polygon) const
Compute the overlap of this polygon and the other polygon if any.
Definition polygon2d.cc:432

◆ ComputeOverlap()

bool apollo::common::math::Polygon2d::ComputeOverlap ( const Polygon2d other_polygon,
Polygon2d *const  overlap_polygon 
) const

Compute the overlap of this polygon and the other polygon if any.

Note: this function only works for computing overlap between two convex polygons.

参数
other_polygonThe target polygon. To compute its overlap with this polygon.
overlap_polygonThe overlapped polygon.
Ifthere is an overlapped polygon.

在文件 polygon2d.cc432 行定义.

433 {
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) {
440 return false;
441 }
442 }
443 return ComputeConvexHull(points, overlap_polygon, false);
444}
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
static bool ClipConvexHull(const LineSegment2d &line_segment, std::vector< Vec2d > *const points)
Definition polygon2d.cc:393
#define ACHECK(cond)
Definition log.h:80

◆ Contains() [1/2]

bool apollo::common::math::Polygon2d::Contains ( const LineSegment2d line_segment) const

Check if the polygon contains a line segment.

参数
line_segmentThe target line segment. To check if the polygon contains it.
返回
Whether the polygon contains the line segment or not.

在文件 polygon2d.cc252 行定义.

252 {
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}
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 IsPointIn(const Vec2d &point) const
Check if a point is within the polygon.
Definition polygon2d.cc:209

◆ Contains() [2/2]

bool apollo::common::math::Polygon2d::Contains ( const Polygon2d polygon) const

Check if the polygon contains another polygon.

参数
polygonThe target polygon. To check if this polygon contains it.
返回
Whether this polygon contains another polygon or not.

在文件 polygon2d.cc274 行定义.

274 {
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}
const std::vector< LineSegment2d > & line_segments() const
Get the edges of the polygon.
Definition polygon2d.h:71

◆ DebugString()

std::string apollo::common::math::Polygon2d::DebugString ( ) const

Get a string containing essential information about the polygon for debugging purpose.

返回
Essential information about the polygon for debugging purpose.

在文件 polygon2d.cc774 行定义.

774 {
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}

◆ DistanceSquareTo()

double apollo::common::math::Polygon2d::DistanceSquareTo ( const Vec2d point) const

Compute the square of distance from a point to the polygon.

If the point is within the polygon, return 0. Otherwise, this square of distance is the minimal square of distance from the point to the edges of the polygon.

参数
pointThe point to compute whose square of distance to the polygon.
返回
The square of distance from the point to the polygon.

在文件 polygon2d.cc74 行定义.

74 {
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}
double DistanceSquareTo(const Vec2d &point) const
Compute the square of distance from a point to the polygon.
Definition polygon2d.cc:74

◆ DistanceTo() [1/7]

double apollo::common::math::Polygon2d::DistanceTo ( const Box2d box) const

Compute the distance from a box to the polygon.

If the box is within the polygon, or it has overlap with the polygon, return 0. Otherwise, this distance is the minimal distance among the distances from the edges of the box to the polygon.

参数
boxThe box to compute whose distance to the polygon.
返回
The distance from the box to the polygon.

在文件 polygon2d.cc153 行定义.

153 {
154 CHECK_GE(points_.size(), 3U);
155 return DistanceTo(Polygon2d(box));
156}
double DistanceTo(const Vec2d &point) const
Compute the distance from a point to the polygon.
Definition polygon2d.cc:45

◆ DistanceTo() [2/7]

double apollo::common::math::Polygon2d::DistanceTo ( const LineSegment2d line_segment) const

Compute the distance from a line segment to the polygon.

If the line segment is within the polygon, or it has intersect with the polygon, return 0. Otherwise, this distance is the minimal distance between the distances from the two ends of the line segment to the polygon.

参数
line_segmentThe line segment to compute whose distance to the polygon.
返回
The distance from the line segment to the polygon.

在文件 polygon2d.cc87 行定义.

87 {
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}

◆ DistanceTo() [3/7]

double apollo::common::math::Polygon2d::DistanceTo ( const LineSegment2d line_segment,
Vec2d polygon_closest_point,
Vec2d segment_closest_point 
) const

Compute the distance from a line segment to the polygon.

If the line segment is within the polygon, or it has intersect with the polygon, return 0. Otherwise, this distance is the minimal distance between the distances from the two ends of the line segment to the polygon.

参数
line_segmentThe line segment to compute whose distance to the polygon.
polygon_closest_pointOutput parameter that returns the closest point on the polygon to the line segment.
segment_closest_pointOutput parameter that returns the closest point on the line segment to the polygon. If the line segment is within the polygon,
返回
The distance from the line segment to the polygon.

在文件 polygon2d.cc110 行定义.

112 {
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}

◆ DistanceTo() [4/7]

double apollo::common::math::Polygon2d::DistanceTo ( const Polygon2d polygon) const

Compute the distance from another polygon to the polygon.

If the other polygon is within this polygon, or it has overlap with this polygon, return 0. Otherwise, this distance is the minimal distance among the distances from the edges of the other polygon to this polygon.

参数
polygonThe polygon to compute whose distance to this polygon.
返回
The distance from the other polygon to this polygon.

在文件 polygon2d.cc158 行定义.

158 {
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}

◆ DistanceTo() [5/7]

double apollo::common::math::Polygon2d::DistanceTo ( const Polygon2d polygon,
Vec2d self_closest_point,
Vec2d other_closest_point 
) const

Compute the distance from another polygon to the polygon.

If the other polygon is within this polygon, or it has overlap with this polygon, return 0. Otherwise, this distance is the minimal distance among the distances from the edges of the other polygon to this polygon.

参数
polygonThe polygon to compute whose distance to this polygon.
self_closest_pointOutput parameter that returns the closest point on this polygon to the other polygon.
other_closest_pointOutput parameter that returns the closest point on the other polygon to this polygon.
返回
The distance from the other polygon to this polygon.

在文件 polygon2d.cc175 行定义.

177 {
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}

◆ DistanceTo() [6/7]

double apollo::common::math::Polygon2d::DistanceTo ( const Vec2d point) const

Compute the distance from a point to the polygon.

If the point is within the polygon, return 0. Otherwise, this distance is the minimal distance from the point to the edges of the polygon.

参数
pointThe point to compute whose distance to the polygon.
返回
The distance from the point to the polygon.

在文件 polygon2d.cc45 行定义.

45 {
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}

◆ DistanceTo() [7/7]

double apollo::common::math::Polygon2d::DistanceTo ( const Vec2d point,
Vec2d closest_point 
) const

Compute the distance from a point to the polygon.

If the point is within the polygon, return 0. Otherwise, this distance is the minimal distance from the point to the edges of the polygon.

参数
pointThe point to compute whose distance to the polygon.
closest_pointAn output parameter that returns the coordinates of the closest point on the polygon to the input point
返回
The distance from the point to the polygon.

在文件 polygon2d.cc57 行定义.

57 {
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}

◆ DistanceToBoundary()

double apollo::common::math::Polygon2d::DistanceToBoundary ( const Vec2d point) const

Compute the distance from a point to the boundary of the polygon.

This distance is equal to the minimal distance from the point to the edges of the polygon.

参数
pointThe point to compute whose distance to the polygon.
返回
The distance from the point to the polygon's boundary.

在文件 polygon2d.cc194 行定义.

194 {
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}

◆ ExpandByDistance()

Polygon2d apollo::common::math::Polygon2d::ExpandByDistance ( const double  distance) const

Expand this polygon by a distance.

参数
distanceThe specified distance. To expand this polygon by it.
返回
The polygon after expansion.

在文件 polygon2d.cc683 行定义.

683 {
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}
double WrapAngle(const double angle)
Wrap angle to [0, 2 * PI).
Definition math_utils.cc:48
constexpr float kMinAngle
Definition lane_object.h:31

◆ ExtremePoints()

void apollo::common::math::Polygon2d::ExtremePoints ( const double  heading,
Vec2d *const  first,
Vec2d *const  last 
) const

Get the extreme points along a heading direction.

参数
headingThe specified heading.
firstThe point on the boundary of this polygon with the minimal projection onto the heading direction.
lastThe point on the boundary of this polygon with the maximal projection onto the heading direction.

在文件 polygon2d.cc579 行定义.

580 {
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}

◆ GetAllOverlaps()

std::vector< LineSegment2d > apollo::common::math::Polygon2d::GetAllOverlaps ( const LineSegment2d line_segment) const

Get all overlapped line segments of a line segment and this polygon.

There are possibly multiple overlapped line segments if this polygon is not convex.

参数
line_segmentThe target line segment. To get its all overlapped line segments with this polygon.
返回
A group of overlapped line segments.

在文件 polygon2d.cc525 行定义.

526 {
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}

◆ GetAllVertices() [1/2]

std::vector< Vec2d > apollo::common::math::Polygon2d::GetAllVertices ( ) const

Get all vertices of the polygon

在文件 polygon2d.cc523 行定义.

523{ return points_; }

◆ GetAllVertices() [2/2]

void apollo::common::math::Polygon2d::GetAllVertices ( std::vector< Vec2d > *const  vertices) const

Get all vertices of the polygon

参数
Allvertices of the polygon

在文件 polygon2d.cc516 行定义.

516 {
517 if (vertices == nullptr) {
518 return;
519 }
520 *vertices = points_;
521}

◆ GetOverlap()

bool apollo::common::math::Polygon2d::GetOverlap ( const LineSegment2d line_segment,
Vec2d *const  first,
Vec2d *const  last 
) const

Get the overlap of a line segment and this polygon.

If they have overlap, output the two ends of the overlapped line segment.

参数
line_segmentThe target line segment. To get its overlap with this polygon.
firstFirst end of the overlapped line segment.
secondSecond end of the overlapped line segment.
返回
If the target line segment has overlap with this polygon.

在文件 polygon2d.cc474 行定义.

475 {
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}

◆ HasOverlap() [1/2]

bool apollo::common::math::Polygon2d::HasOverlap ( const LineSegment2d line_segment) const

Check if a line segment has overlap with this polygon.

参数
line_segmentThe target line segment. To check if it has overlap with this polygon.
返回
Whether the target line segment has overlap with this polygon or not.

在文件 polygon2d.cc456 行定义.

456 {
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}

◆ HasOverlap() [2/2]

bool apollo::common::math::Polygon2d::HasOverlap ( const Polygon2d polygon) const

Check if this polygon has overlap with another polygon.

参数
polygonThe target polygon. To check if it has overlap with this polygon.
返回
If this polygon has overlap with another polygon.

在文件 polygon2d.cc228 行定义.

228 {
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}
bool HasOverlap(const LineSegment2d &line_segment) const
Check if a line segment has overlap with this polygon.
Definition polygon2d.cc:456

◆ is_convex()

bool apollo::common::math::Polygon2d::is_convex ( ) const
inline

Check if the polygon is convex.

返回
Whether the polygon is convex or not.

在文件 polygon2d.h85 行定义.

85{ return is_convex_; }

◆ IsPointIn()

bool apollo::common::math::Polygon2d::IsPointIn ( const Vec2d point) const

Check if a point is within the polygon.

参数
pointThe target point. To check if it is within the polygon.
返回
Whether a point is within the polygon or not.

在文件 polygon2d.cc209 行定义.

209 {
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}
bool IsPointOnBoundary(const Vec2d &point) const
Check if a point is on the boundary of the polygon.
Definition polygon2d.cc:202

◆ IsPointOnBoundary()

bool apollo::common::math::Polygon2d::IsPointOnBoundary ( const Vec2d point) const

Check if a point is on the boundary of the polygon.

参数
pointThe target point. To check if it is on the boundary of the polygon.
返回
Whether a point is on the boundary of the polygon or not.

在文件 polygon2d.cc202 行定义.

202 {
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}

◆ line_segments()

const std::vector< LineSegment2d > & apollo::common::math::Polygon2d::line_segments ( ) const
inline

Get the edges of the polygon.

返回
The edges of the polygon.

在文件 polygon2d.h71 行定义.

71 {
72 return line_segments_;
73 }

◆ max_x()

double apollo::common::math::Polygon2d::max_x ( ) const
inline

在文件 polygon2d.h366 行定义.

366{ return max_x_; }

◆ max_y()

double apollo::common::math::Polygon2d::max_y ( ) const
inline

在文件 polygon2d.h368 行定义.

368{ return max_y_; }

◆ min_x()

double apollo::common::math::Polygon2d::min_x ( ) const
inline

在文件 polygon2d.h365 行定义.

365{ return min_x_; }

◆ min_y()

double apollo::common::math::Polygon2d::min_y ( ) const
inline

在文件 polygon2d.h367 行定义.

367{ return min_y_; }

◆ MinAreaBoundingBox()

Box2d apollo::common::math::Polygon2d::MinAreaBoundingBox ( ) const

Get the bounding box with the minimal area.

返回
The bounding box with the minimal area.

在文件 polygon2d.cc624 行定义.

624 {
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}
Box2d BoundingBoxWithHeading(const double heading) const
Get the bound box according to a heading.
Definition polygon2d.cc:605
double area() const
Get the area of the polygon.
Definition polygon2d.h:91

◆ MinLineSegment()

LineSegment2d apollo::common::math::Polygon2d::MinLineSegment ( ) const

在文件 polygon2d.cc763 行定义.

763 {
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}

◆ Next()

int apollo::common::math::Polygon2d::Next ( int  at) const
protected

在文件 polygon2d.cc289 行定义.

289{ return at >= num_points_ - 1 ? 0 : at + 1; }

◆ num_points()

int apollo::common::math::Polygon2d::num_points ( ) const
inline

Get the number of vertices of the polygon.

返回
The number of vertices of the polygon.

在文件 polygon2d.h79 行定义.

79{ return num_points_; }

◆ points()

const std::vector< Vec2d > & apollo::common::math::Polygon2d::points ( ) const
inline

Get the vertices of the polygon.

返回
The vertices of the polygon.

在文件 polygon2d.h65 行定义.

65{ return points_; }

◆ PolygonExpandByDistance()

Polygon2d apollo::common::math::Polygon2d::PolygonExpandByDistance ( const double  distance) const

在文件 polygon2d.cc713 行定义.

713 {
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}

◆ Prev()

int apollo::common::math::Polygon2d::Prev ( int  at) const
protected

在文件 polygon2d.cc291 行定义.

291{ return at == 0 ? num_points_ - 1 : at - 1; }

类成员变量说明

◆ area_

double apollo::common::math::Polygon2d::area_ = 0.0
protected

在文件 polygon2d.h383 行定义.

◆ is_convex_

bool apollo::common::math::Polygon2d::is_convex_ = false
protected

在文件 polygon2d.h382 行定义.

◆ line_segments_

std::vector<LineSegment2d> apollo::common::math::Polygon2d::line_segments_
protected

在文件 polygon2d.h381 行定义.

◆ max_x_

double apollo::common::math::Polygon2d::max_x_ = 0.0
protected

在文件 polygon2d.h385 行定义.

◆ max_y_

double apollo::common::math::Polygon2d::max_y_ = 0.0
protected

在文件 polygon2d.h387 行定义.

◆ min_x_

double apollo::common::math::Polygon2d::min_x_ = 0.0
protected

在文件 polygon2d.h384 行定义.

◆ min_y_

double apollo::common::math::Polygon2d::min_y_ = 0.0
protected

在文件 polygon2d.h386 行定义.

◆ num_points_

int apollo::common::math::Polygon2d::num_points_ = 0
protected

在文件 polygon2d.h380 行定义.

◆ points_

std::vector<Vec2d> apollo::common::math::Polygon2d::points_
protected

在文件 polygon2d.h379 行定义.


该类的文档由以下文件生成: