平面几何处理方法——点线关系判断函数

系列文章目录

暂无

---

---

前言

平面几何处理，主要是线线关系，点线关系的一些函数

---

提示：本文代码为学习记录，仅供参考，是否可用自行测试

一、线线关系

1.判断两线平行

输入为两条线的左右端点坐标，可选平行阈值k，默认为5°，也可能反向平行

其实就是计算两向量的夹角，代码如下（C++）
输入：两直线左右端点，平行角度阈值k
输出：bool

bool IsLinesParallel(Eigen::Vector2f& line1_left, Eigen::Vector2f& line1_right,
                     Eigen::Vector2f& line2_left, Eigen::Vector2f& line2_right, float k = 5) {
  float cos_theta = fabs((line1_right - line1_left).dot(line2_right - line2_left) /
                         (line1_right - line1_left).norm() / (line2_right - line2_left).norm());
  return cos_theta > std::cos(k * M_PI / 180.0) || cos_theta < std::cos((180 - k) * M_PI / 180.0);
}

2.判断两线垂直

输入为两条线的左右端点坐标，可选角度阈值k，默认为85°，即范围为85~95°

输入：两直线左右端点，垂直角度阈值k
输出：bool

bool IsLinesVertical(Eigen::Vector2f& line1_left, Eigen::Vector2f& line1_right,
                     Eigen::Vector2f& line2_left, Eigen::Vector2f& line2_right, float k = 85) {
  float cos_theta = fabs((line1_right - line1_left).dot(line2_right - line2_left) /
                         (line1_right - line1_left).norm() / (line2_right - line2_left).norm());
  return cos_theta > std::cos(k * M_PI / 180.0) || cos_theta < std::cos((180 - k) * M_PI / 180.0);
}

3.计算两向量的交点

原理是两向量交点坐标可视为一个向量向另一向量上投影的缩放，计算出缩放比例 t 便可求出交点坐标（公式自行推导），这里的交点可能在向量端点范围外

输入：两向量左右端点，待求交点，交点比例t
输出：bool

bool CalcLinesIntersection(Eigen::Vector2f& line1_left, Eigen::Vector2f& line1_right,
                           Eigen::Vector2f& line2_left, Eigen::Vector2f& line2_right,
                           Eigen::Vector2f& intersection, float& t) {
  if (IsLinesParallel(line1_left, line1_right, line2_left, line2_right)) {
    return false;
  }

  float a = (line1_right.x() - line1_left.x()) * (line2_right.y() - line2_left.y()) -
            (line1_right.y() - line1_left.y()) * (line2_right.x() - line2_left.x());
  float b = (line2_left.x() - line1_left.x()) * (line2_right.y() - line2_left.y()) -
            (line2_left.y() - line1_left.y()) * (line2_right.x() - line2_left.x());
  float t = b / a;
  intersection = line1_left + t * (line1_right - line1_left);

  if (fabs(a) < 1e-3) {
    return false;
  }
  return true;
}

4.计算两线段的交点

原理与计算向量交点相同，只是需要注意线段交点需在两端点之间，或者可选在端点附近阈值 k 内，不然的话计算交点比例超出（0, 1）范围则视为没有交点。

输入：两线段左右端点，待求交点，范围阈值k
输出：bool

bool CalcSegmentsIntersection(Eigen::Vector2f& segment1_left, Eigen::Vector2f& segment1_right,
                              Eigen::Vector2f& segment2_left, Eigen::Vector2f& segment2_right,
                              Eigen::Vector2f& intersection, float k = 0.2) {
  float t1, t2;
  if (!CalcLinesIntersection(segment1_left, segment1_right, segment2_left, segment2_right,
                             intersection, t1) ||
      !CalcLinesIntersection(segment2_left, segment2_right, segment1_left, segment1_right,
                             intersection, t2)) {
    return false;
  }

  if ((intersection - segment1_left).norm() < (segment1_right - segment1_left).norm() + k &&
      (intersection - segment1_right).norm() < (segment1_right - segment1_left).norm() + k &&
      (intersection - segment2_left).norm() < (segment2_right - segment2_left).norm() + k &&
      (intersection - segment2_right).norm() < (segment2_right - segment2_left).norm() + k) {
    return true;
  }

  if (t1 < 0 || t1 > 1 || t2 < 0 || t2 > 1) {
    return false;
  }
  return true;
}

二、点线关系

1.计算点到线段投影长度

可视为求一向量到另一向量的投影，利用点乘，a b cos0

输入：点坐标，线段两端点
输出：投影长度

float CalcProjecLength(Eigen::Vector2f& point, Eigen::Vector2f& segment_left,
                       Eigen::Vector2f& segment_right) {
  return (point - segment_left).dot((segment_right - segment_left).normalized());
}

2.计算点到线段距离

可视为求一向量到另一向量的投影，利用叉乘，a b sin0

输入：点坐标，线段两端点
输出：距离长度

float CalcPointDistance(Eigen::Vector2f& point, Eigen::Vector2f& segment_left,
                              Eigen::Vector2f& segment_right) {
  float a = (segment_right.x() - segment_left.x()) * (point.y() - segment_left.y()) -
               (segment_right.y() - segment_left.y()) * (point.x() - segment_left.x());
  return std::fabs(a) / (segment_right - segment_left).norm();
}

3.判断点与线段关系

计算点在线段方向上的投影，可能在线段左边、中间、右边，对应值-1、0、1

输入：点坐标，线段两端点，范围阈值k
输出：int代表点与线的关系

int CalcPointStatus(Eigen::Vector2f& point, Eigen::Vector2f& segment_left,
                    Eigen::Vector2f& segment_right, float k) {
  float length = CalcProjectLength(point, segment_left, segment_right);
  if (length < -k) {
    return -1;
  } else if (length > (segment_right - segment_left).norm() + k) {
    return 1;
  }
  return 0;
}

4.判断点在线段上

分别判断点的投影是否在线段外，到线段距离是否满足要求，因此有两个阈值

输入：点坐标，线段两端点，长度范围阈值，距离阈值
输出：bool

bool IsPointOnSegment(Eigen::Vector2f& point, Eigen::Vector2f& segment_left,
                      Eigen::Vector2f& segment_right, float length, float distance) {
  if (CalcPointStatus(point, segment_left, segment_right, length) != 0) {
    return false;
  }
  if (CalcPointDistance(point, segment_left, segment_right) > distance) {
    return false;
  }
  return true;
}

5.点在线段上对应的坐标插值

遍历线段上的相邻点，判断点与该片段的关系（范围和距离），保存距离最近的index

输入：点坐标，直线点链，插值索引，插值点坐标，最近索引
输出：bool

bool InterploatePointForLine(Eigen::Vector2f& point, std::vector<Eigen::Vector2f>& line,
                             int& target_index, Eigen::Vector2f& intersection, int& nearest_index) {
  if (line.size() < 2) {
    return false;
  }

  float min_dis = 50.0f;
  for (int i = 0; i < line.size() - 1; ++i) {
    if (CalcPointStatus(point, line[i], line[i + 1], 0.2) == 0) {
      float tmp = CalcPointDistance(point, line[i], line[i + 1]);
      if (tmp < min_dis) {
        min_dis = tmp;
        target_index = i;
        intersection = line[i] + (line[i + 1] - line[i]).normalized() * CalcProjectLength(point, line[i], line[i + 1]);
        nearest_index = (point - line[i + 1]).norm() < (point - line[i]).norm() ? i + 1 : i;
      }
    }
  }
  return min_dis > 50.0f ? false : true;
}

---

总结

平面几何的点线处理方法，记录自用。