平面几何处理方法——点链对应距离计算

系列文章目录

平面几何处理方法一

---

---

前言

计算两条由点链构成的线的距离，以其中一条线为基准，为另一条线上的点计算距离，适用于无固定形状的点链（无弯折），部分函数来自平面几何处理方法一

---

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

一、逻辑简述

若两条点链分别为左线和右线，、以左线为基准，遍历相邻的两点，若右线上的某点的投影在左线两点之间，则计算该右线点到左线两点构成的线段的距离，保存右线点、左线前一点的索引以及计算出的距离。

二、代码

前置函数：
CalcPointStatus（）判断点和线段的状态：在线段前、在线段中、在线段后
CalcPointDistance（）计算点到线的距离

输入：两条点链，tuple数组（点索引，距离）
输出：bool

bool CalcPointLineDis(std::vector<Eigen::Vector2f>& left_line,
                      std::vector<Eigen::Vector2f>& right_line,
                      std::vector<std::tuple<int, int, float>>& dis) {
  if (left_line.size() < 2 || right_line.size() < 2) {
    return false;
  }

  int left_index = 1, right_index = 0;
  while (left_index < left_point_line.size()) {
    if ((left_line[left_index] - left_line[left_index - 1]).norm() < 0.1) {
      ++left_index;
    } else {
      break;
    }
  }

  while (left_index < left_line.size() && right_index < right_line.size()) {
    int point_status = CalcPointStatus(right_line[right_index], left_line[left_index - 1],
                                       left_line[left_index], 0.2);
    if (point_status == -1) {
      ++right_index;
    } else {
      break;
    }
  }

  for (; right_index < right_line.size(); ++right_index) {
    while (left_index < left_line.size()) {
      if ((left_line[left_index - 1] - left_line[left_index]).norm() < 0.1) {
        ++left_index;
        continue;
      }
      int point_status = CalcPointStatus(right_line[right_index], left_line[left_index - 1],
                                         left_line[left_index], 0.2);
      if (point_status == 1) {
        ++left_index;
      } else {
        break;
      }
    }
    if (left_index >= left_line.size() ||
        (left_point_line[left_index - 1] - left_point_line[left_index]).norm() < 0.1) {
      break;
    }

    int point_status = CalcPointStatus(right_line[right_index], left_line[left_index - 1],
                                       left_line[left_index], 0.2);
    if (point_status == 1) {
      return false;
    } else {
      dis.emplace_back(left_index - 1, right_index,
                       CalcPointDistance(right_line[right_index], left_line[left_index - 1],
                                         left_line[left_index]));
    }
  }

  if (dis.empty()) {
    return false;
  }
  return true;
}

---