24#include "absl/strings/str_cat.h"
25#include "absl/strings/str_join.h"
41 : points_(
std::move(points)) {
50 double distance = std::numeric_limits<double>::infinity();
62 double distance = std::numeric_limits<double>::infinity();
63 Vec2d closest_point_tmp;
68 closest_point = closest_point_tmp;
79 double distance_sqr = std::numeric_limits<double>::infinity();
97 return poly_seg.HasIntersect(line_segment);
111 Vec2d& polygon_closest_point,
112 Vec2d& segment_closest_point)
const {
114 segment_closest_point = line_segment.
start();
124 return poly_seg.HasIntersect(line_segment);
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;
137 segment_closest_point = line_segment.
end();
138 polygon_closest_point = end_closest_point;
139 distance = distance_end;
141 Vec2d closest_point_tmp;
144 if (dis < distance) {
146 segment_closest_point = closest_point_tmp;
147 polygon_closest_point =
points_[i];
168 double distance = std::numeric_limits<double>::infinity();
176 Vec2d& self_closest_point,
177 Vec2d& other_closest_point)
const {
180 double distance = std::numeric_limits<double>::infinity();
181 Vec2d segment_closest_point_tmp, polygon_closest_point_tmp;
184 polygon_closest_point_tmp, segment_closest_point_tmp);
185 if (dis < distance) {
187 self_closest_point = segment_closest_point_tmp;
188 other_closest_point = polygon_closest_point_tmp;
195 double distance = std::numeric_limits<double>::infinity();
206 [&](
const LineSegment2d &poly_seg) { return poly_seg.IsPointIn(point); });
244 for (
int i = 0; i < polygon.
num_points(); ++i) {
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();
285 return Contains(line_segment);
332 for (
const auto &point :
points_) {
341 Polygon2d *
const polygon,
bool check_area) {
342 CHECK_NOTNULL(polygon);
343 const int n =
static_cast<int>(
points.size());
347 std::vector<int> sorted_indices(n);
348 for (
int i = 0; i < n; ++i) {
349 sorted_indices[i] = i;
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) {
359 return pt1.y() < pt2.y();
362 std::vector<int> results;
365 for (
int i = 0; i < n + n; ++i) {
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]],
377 results.push_back(idx);
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]]);
389 *polygon = Polygon2d(result_points, check_area);
394 std::vector<Vec2d> *
const points) {
398 CHECK_NOTNULL(points);
399 const size_t n = points->size();
403 std::vector<double> prod(n);
404 std::vector<int> side(n);
405 for (
size_t i = 0; i < n; ++i) {
410 side[i] = ((prod[i] < 0) ? -1 : 1);
414 std::vector<Vec2d> new_points;
415 for (
size_t i = 0; i < n; ++i) {
417 new_points.push_back((*points)[i]);
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));
428 points->swap(new_points);
429 return points->size() >= 3U;
432bool Polygon2d::ComputeOverlap(
const Polygon2d &other_polygon,
433 Polygon2d *
const overlap_polygon)
const {
434 CHECK_GE(points_.size(), 3U);
435 CHECK_NOTNULL(overlap_polygon);
437 std::vector<Vec2d> points = other_polygon.
points();
438 for (
int i = 0; i < num_points_; ++i) {
439 if (!ClipConvexHull(line_segments_[i], &points)) {
443 return ComputeConvexHull(points, overlap_polygon,
false);
446double Polygon2d::ComputeIoU(
const Polygon2d &other_polygon)
const {
448 if (!ComputeOverlap(other_polygon, &overlap_polygon)) {
451 double intersection_area = overlap_polygon.
area();
452 double union_area = area_ + other_polygon.
area() - overlap_polygon.
area();
453 return intersection_area / union_area;
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_)) {
465 if (std::any_of(line_segments_.begin(), line_segments_.end(),
467 return poly_seg.HasIntersect(line_segment);
475 Vec2d *
const first,
Vec2d *
const last)
const {
476 CHECK_GE(points_.size(), 3U);
477 CHECK_NOTNULL(first);
481 if (!IsPointIn(line_segment.
start())) {
484 *first = line_segment.
start();
485 *last = line_segment.
start();
489 double min_proj = line_segment.
length();
491 if (IsPointIn(line_segment.
start())) {
492 *first = line_segment.
start();
495 if (IsPointIn(line_segment.
end())) {
496 *last = line_segment.
end();
497 max_proj = line_segment.
length();
499 for (
const auto &poly_seg : line_segments_) {
501 if (poly_seg.GetIntersect(line_segment, &pt)) {
503 if (proj < min_proj) {
507 if (proj > max_proj) {
516void Polygon2d::GetAllVertices(std::vector<Vec2d> *
const vertices)
const {
517 if (vertices ==
nullptr) {
523std::vector<Vec2d> Polygon2d::GetAllVertices()
const {
return points_; }
525std::vector<LineSegment2d> Polygon2d::GetAllOverlaps(
527 CHECK_GE(points_.size(), 3U);
530 std::vector<LineSegment2d> overlaps;
531 if (IsPointIn(line_segment.
start())) {
532 overlaps.push_back(line_segment);
536 std::vector<double> projections;
537 if (IsPointIn(line_segment.
start())) {
538 projections.push_back(0.0);
540 if (IsPointIn(line_segment.
end())) {
541 projections.push_back(line_segment.
length());
543 for (
const auto &poly_seg : line_segments_) {
545 if (poly_seg.GetIntersect(line_segment, &pt)) {
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];
557 const Vec2d reference_point =
558 line_segment.
start() +
560 if (!IsPointIn(reference_point)) {
563 if (overlaps.empty() ||
565 overlaps.emplace_back(start_proj, end_proj);
567 overlaps.back().second = end_proj;
570 std::vector<LineSegment2d> overlap_line_segments;
571 for (
const auto &overlap : overlaps) {
572 overlap_line_segments.emplace_back(
576 return overlap_line_segments;
579void Polygon2d::ExtremePoints(
const double heading,
Vec2d *
const first,
580 Vec2d *
const last)
const {
581 CHECK_GE(points_.size(), 3U);
582 CHECK_NOTNULL(first);
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) {
594 if (proj > max_proj) {
602 return AABox2d({min_x_, min_y_}, {max_x_, max_y_});
605Box2d Polygon2d::BoundingBoxWithHeading(
const double heading)
const {
606 CHECK_GE(points_.size(), 3U);
607 const Vec2d direction_vec = Vec2d::CreateUnitVec2d(heading);
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);
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);
624Box2d Polygon2d::MinAreaBoundingBox()
const {
625 CHECK_GE(points_.size(), 3U);
628 ComputeConvexHull(points_, &convex_polygon);
632 double min_area = std::numeric_limits<double>::infinity();
633 double min_area_at_heading = 0.0;
637 for (
int i = 0; i < num_points_; ++i) {
638 const auto &line_segment = line_segments_[i];
640 double min_proj = line_segment.ProjectOntoUnit(points_[left_most]);
641 while ((proj = line_segment.ProjectOntoUnit(points_[Prev(left_most)])) <
644 left_most = Prev(left_most);
646 while ((proj = line_segment.ProjectOntoUnit(points_[Next(left_most)])) <
649 left_most = Next(left_most);
651 double max_proj = line_segment.ProjectOntoUnit(points_[right_most]);
652 while ((proj = line_segment.ProjectOntoUnit(points_[Prev(right_most)])) >
655 right_most = Prev(right_most);
657 while ((proj = line_segment.ProjectOntoUnit(points_[Next(right_most)])) >
660 right_most = Next(right_most);
663 double max_prod = line_segment.ProductOntoUnit(points_[top_most]);
664 while ((prod = line_segment.ProductOntoUnit(points_[Prev(top_most)])) >
667 top_most = Prev(top_most);
669 while ((prod = line_segment.ProductOntoUnit(points_[Next(top_most)])) >
672 top_most = Next(top_most);
674 const double area = max_prod * (max_proj - min_proj);
675 if (area < min_area) {
677 min_area_at_heading = line_segment.heading();
680 return BoundingBoxWithHeading(min_area_at_heading);
683Polygon2d Polygon2d::ExpandByDistance(
const double distance)
const {
686 ComputeConvexHull(points_, &convex_polygon);
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);
697 points.push_back(points_[i] +
698 Vec2d::CreateUnitVec2d(start_angle) * distance);
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);
709 ACHECK(ComputeConvexHull(points, &new_polygon));
713Polygon2d Polygon2d::PolygonExpandByDistance(
const double distance)
const {
716 ComputeConvexHull(points_, &convex_polygon);
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);
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);
734 double l = distance / sqrt((1 - (v1x * v2x + v1y * v2y)) / 2);
736 double vx = v1x + v2x;
737 double vy = v1y + v2y;
738 double n = l / sqrt(vx * vx + vy * vy);
743 Vec2d point(vx + points_[i].x(), vy + points_[i].y());
744 points.push_back(point);
747 ACHECK(ComputeConvexHull(points, &new_polygon));
751void Polygon2d::CalculateVertices(
const Vec2d &shift_vec) {
752 for (
size_t i = 0; i < num_points_; ++i) {
753 points_[i] += shift_vec;
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_);
764 double min_length_of_line_segment = 10e3;
766 for (
auto &line_segment_ : line_segments_) {
767 if (line_segment_.length() < min_length_of_line_segment) {
768 line_segment = line_segment_;
774std::string Polygon2d::DebugString()
const {
775 return absl::StrCat(
"polygon2d ( num_points = ", num_points_,
" points = (",
777 " ) ", is_convex_ ?
"convex" :
"non-convex",
778 " area = ", area_,
" )");
Implements a class of (undirected) axes-aligned bounding boxes in 2-D.
Rectangular (undirected) bounding box in 2-D.
void GetAllCorners(std::vector< Vec2d > *const corners) const
Getter of the corners of the box
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.
std::vector< LineSegment2d > GetAllOverlaps(const LineSegment2d &line_segment) const
Get all overlapped line segments of a line segment and this polygon.
bool HasOverlap(const LineSegment2d &line_segment) const
Check if a line segment has overlap with this polygon.
std::vector< Vec2d > points_
bool IsPointIn(const Vec2d &point) const
Check if a point is within the polygon.
static bool ComputeConvexHull(const std::vector< Vec2d > &points, Polygon2d *const polygon, bool check_area=true)
Compute the convex hull of a group of points.
bool Contains(const LineSegment2d &line_segment) const
Check if the polygon contains a line segment.
Polygon2d()=default
Empty constructor.
Polygon2d ExpandByDistance(const double distance) const
Expand this polygon by a distance.
Box2d MinAreaBoundingBox() const
Get the bounding box with the minimal area.
double DistanceTo(const Vec2d &point) const
Compute the distance from a point to the polygon.
const std::vector< LineSegment2d > & line_segments() const
Get the edges of the polygon.
const std::vector< Vec2d > & points() const
Get the vertices of the polygon.
double DistanceSquareTo(const Vec2d &point) const
Compute the square of distance from a point to the polygon.
void BuildFromPoints(bool check_area=true)
std::vector< LineSegment2d > line_segments_
bool IsPointOnBoundary(const Vec2d &point) const
Check if a point is on the boundary of the polygon.
int num_points() const
Get the number of vertices of the polygon.
Polygon2d PolygonExpandByDistance(const double distance) const
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.
Implements a class of 2-dimensional vectors.
double InnerProd(const Vec2d &other) const
Returns the inner product between these two Vec2d.
double y() const
Getter for y component
double DistanceTo(const Vec2d &other) const
Returns the distance to the given vector
double x() const
Getter for x component
double CrossProd(const Vec2d &other) const
Returns the "cross" product between these two Vec2d (non-standard).
Math-related util functions.
constexpr double kMathEpsilon
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.
double WrapAngle(const double angle)
Wrap angle to [0, 2 * PI).
Define the Polygon2d class.
Some string util functions.