C++-点到点、点到线、共线等常用计算

1. 一点到另外两点组成直线的距离

已知点p /p1/p2,求p到p1 p2的距离
根据点到直线的距离公式：d = (Ax0 + By0 + C) / sqrt(A * A + B * B)
A = y2 - y1
B = x1 - x2；
C = x2 * y1 - x1 * y2;

程序实现

//定义Point2f结构体  
struct Point2f
{
	float x;
	float y;
};

// 定义直线参数结构体  
struct LinePara
{
	float k;
	float b;
}
//距离计算
float NavMath::PointToLineDis(Point2f p, Point2f p1, Point2f p2)
{
	float A = p2.y - p1.y;
	float B = p1.x - p2.x;
	float C = p2.x * p1.y - p1.x * p2.y;
	float dis = (A * p.x + B * p.y + C) / sqrt(A * A + B * B);
	return dis;
}

2.获取平面两点间的距离

两点组成向量的模即为两点间的距离

//使用Eigen库，两点组成向量的模即为距离
// 获取平面两点之间的距离
double GetTwoPointsDistance(const Eigen::Vector2d &p1,
                            const Eigen::Vector2d &p2) {
  return (p1 - p2).norm();
}

3计算两个向量的夹角

由两个向量计算夹角

// 获取两个向量的夹角
double GetTwoVectorsAngle(const Eigen::Vector2d &v1,
                          const Eigen::Vector2d &v2) {
  double cosine_value = v1.dot(v2) / (v1.norm() * v2.norm());
  // 归约到-1到1,防止出错
//  if (cosine_value > 1)
   // cosine_value = 1;
  //else if (cosine_value < -1)
    //cosine_value = -1;

//弧度
  double value = acos(cosine_value);

  return value;
}

4. 判断三点共线

/检查三点是否共线,以p1为顶点的角度小于某个值来判断

bool IsThreePointsInLine(const Eigen::Vector2d &p1, const Eigen::Vector2d &p2,
                         const Eigen::Vector2d &p3, double radian_deviation) {
  Eigen::Vector2d p1_p2 = p2 - p1;
  Eigen::Vector2d p1_p3 = p3 - p1;
  // 有重合点直接返回true
  if (p1_p2.norm() == 0 || p1_p3.norm() == 0) {
    return true;
  }

  double radian_value_of_p1 = GetTwoVectorsAngle(p1_p2, p1_p3);


  Eigen::Vector2d p3_p1 = p1 - p3;
  Eigen::Vector2d p3_p2 = p2 - p3;
  // 有重合点直接返回true
  if (p3_p1.norm() == 0 || p3_p2.norm() == 0) {
    return true;
  }

  double radian_value_of_p3 = GetTwoVectorsAngle(p3_p1, p3_p2);

  if (radian_value_of_p1 <= radian_deviation &&
      radian_value_of_p3 <= radian_deviation) {
    return true;
  } else {
    return false;
  }
}

TODO 数据类型多用eigen库