多边形交集计算

#include "poly_topo.h"
using namespace std;
// using namespace cv;

// 点集排序
// 若点a大于点b,即点a在点b顺时针方向,返回true,否则返回false
bool PointCompare(const Eigen::Vector2d& a, const Eigen::Vector2d& b,
                  const Eigen::Vector2d& center) {
  if (a.x() >= 0 && b.x() < 0) return true;
  if (a.x() == 0 && b.x() == 0) return a.y() > b.y();
  // 向量OA和向量OB的叉积
  auto det =
      (a.x() - center.x()) * (b.y() - center.y()) - (b.x() - center.x()) * (a.y() - center.y());
  if (det < 0) return true;
  if (det > 0) return false;
  // 向量OA和向量OB共线，以距离判断大小
  auto d1 =
      (a.x() - center.x()) * (a.x() - center.x()) + (a.y() - center.y()) * (a.y() - center.y());
  auto d2 =
      (b.x() - center.x()) * (b.x() - center.y()) + (b.y() - center.y()) * (b.y() - center.y());

  return d1 > d2;
}
// 顺时针方向排序
void ClockwiseSortPoints(std::vector<Eigen::Vector2d>& vPoints) {
  // 计算重心
  Eigen::Vector2d center;
  int count_size = vPoints.size();
  double x = 0, y = 0;
  for (int i = 0; i < count_size; i++) {
    x += vPoints[i].x();
    y += vPoints[i].y();
  }
  center.x() = x / count_size;
  center.y() = y / count_size;

  // 冒泡排序
  for (int i = 0; i < count_size - 1; i++) {
    for (int j = 0; j < count_size - i - 1; j++) {
      if (PointCompare(vPoints[j], vPoints[j + 1], center)) {
        Eigen::Vector2d tmp = vPoints[j];
        vPoints[j] = vPoints[j + 1];
        vPoints[j + 1] = tmp;
      }
    }
  }

  return;
}

// 判断点是否在多边形内部/
//   The function will return YES if the point x,y is inside the polygon, or
//   NO if it is not.  If the point is exactly on the edge of the polygon,
//   then the function may return YES or NO.
bool IsPointInPolygon(const std::vector<Eigen::Vector2d>& poly, const Eigen::Vector2d& pt) {
  int i, j;
  bool c = false;
  int count = poly.size();
  for (i = 0, j = count - 1; i < count; j = i++) {
    if ((((poly[i].y() <= pt.y()) && (pt.y() < poly[j].y())) ||
         ((poly[j].y() <= pt.y()) && (pt.y() < poly[i].y()))) &&
        (pt.x() < (poly[j].x() - poly[i].x()) * (pt.y() - poly[i].y()) /
                        (poly[j].y() - poly[i].y()) +
                    poly[i].x())) {
      c = !c;
    }
  }

  return c;
}

/// 线段相交判断//
// 排斥实验
bool IsRectCross(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, const Eigen::Vector2d& q1,
                 const Eigen::Vector2d& q2) {
  bool ret = min(p1.x(), p2.x()) <= max(q1.x(), q2.x()) &&
             min(q1.x(), q2.x()) <= max(p1.x(), p2.x()) &&
             min(p1.y(), p2.y()) <= max(q1.y(), q2.y()) &&
             min(q1.y(), q2.y()) <= max(p1.y(), p2.y());

  return ret;
}
// 跨立判断
bool crossProduct(const Eigen::Vector2d& A, const Eigen::Vector2d& B,
                  const Eigen::Vector2d& C, const Eigen::Vector2d& D) {
  double cross1 =
      (B.x() - A.x()) * (C.y() - A.y()) - (C.x() - A.x()) * (B.y() - A.y());
  double cross2 =
      (B.x() - A.x()) * (D.y() - A.y()) - (D.x() - A.x()) * (B.y() - A.y());

  if (cross1 * cross2 < 0) {
    return true;
  } else {
    return false;
  }
}

bool GetCrossPoint(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2,
                   const Eigen::Vector2d& q1, const Eigen::Vector2d& q2, long& x, long& y) {
  if (IsRectCross(p1, p2, q1, q2)) {
    if (crossProduct(p1, p2, q1, q2)) {
      // 求交点
      double tmpLeft, tmpRight;
      tmpLeft = (q2.x() - q1.x()) * (p1.y() - p2.y()) - (p2.x() - p1.x()) * (q1.y() - q2.y());
      tmpRight = (p1.y() - q1.y()) * (p2.x() - p1.x()) * (q2.x() - q1.x()) +
                 q1.x() * (q2.y() - q1.y()) * (p2.x() - p1.x()) -
                 p1.x() * (p2.y() - p1.y()) * (q2.x() - q1.x());

      x = tmpRight / tmpLeft;

      tmpLeft = (p1.x() - p2.x()) * (q2.y() - q1.y()) - (p2.y() - p1.y()) * (q1.x() - q2.x());
      tmpRight = p2.y() * (p1.x() - p2.x()) * (q2.y() - q1.y()) +
                 (q2.x() - p2.x()) * (q2.y() - q1.y()) * (p1.y() - p2.y()) -
                 q2.y() * (q1.x() - q2.x()) * (p2.y() - p1.y());
      y = tmpRight / tmpLeft;
      return true;
    }
  }
  return false;
}
/// 线段相交结束//

// 多边形交集
bool PolygonClip(const std::vector<Eigen::Vector2d>& poly1,
                 const std::vector<Eigen::Vector2d>& poly2,
                 std::vector<Eigen::Vector2d>& interPoly) {
  if (poly1.size() < 3 || poly2.size() < 3) {
    return false;
  }

  long x, y;
  // 计算多边形交点
  int count1 = poly1.size();
  int count2 = poly2.size();
  for (int i = 0; i < count1; i++) {
    int poly1_next_idx = (i + 1) % count1;
    for (int j = 0; j < count2; j++) {
      int poly2_next_idx = (j + 1) % count2;
      if (GetCrossPoint(poly1[i], poly1[poly1_next_idx], poly2[j],
                        poly2[poly2_next_idx], x, y)) {
        interPoly.push_back(Eigen::Vector2d(x, y));
      }
    }
  }

  // 计算多边形内部点
  for (int i = 0; i < count1; i++) {
    if (IsPointInPolygon(poly2, poly1[i])) {
      interPoly.push_back(poly1[i]);
    }
  }
  for (int i = 0; i < count2; i++) {
    if (IsPointInPolygon(poly1, poly2[i])) {
      interPoly.push_back(poly2[i]);
    }
  }

  if (interPoly.size() <= 0) return false;

  // 点集排序
  ClockwiseSortPoints(interPoly);

  return true;
}
double culPolygonArea(const std::vector<Eigen::Vector2d>& points) {
  int point_num = points.size();
  if (point_num < 3) return 0.0;
  double s = points[0].y() * (points[point_num - 1].x() - points[1].x());
  for (int i = 1; i < point_num; ++i)
    s += points[i].y() * (points[i - 1].x() - points[(i + 1) % point_num].x());
  return fabs(s / 2.0);
}

bool addOffset(const std::vector<Eigen::Vector2d>& points,
               std::vector<Eigen::Vector2d>& outPts) {
  if (points.size() < 1) {
    return false;
  }
  double minx = std::numeric_limits<double>::max();
  double miny = std::numeric_limits<double>::max();
  for (auto pt : points) {
    minx = minx < pt.x() ? minx : pt.x();
    miny = miny < pt.y() ? miny : pt.y();
  }
  for (auto pt : points) {
    Eigen::Vector2d outpt;
    outpt.x() = pt.x() - minx;
    outpt.y() = pt.y() - miny;
    outPts.push_back(outpt);
  }
  return true;
}