点在平面上的投影坐标
原理
通过平面上随意找一个点与之形成向量,通过向量与平面的法向量点乘得到距离值
代码
/**
* @brief 一个点在面上的映射坐标
*
* @param p 目标点
* @param a
* @param b
* @param c
* @param d
* @param plane_p0 平面上取得的任意一点
* @return common_msgs::Point3d 返回交点坐标
*/
static common_msgs::Point3d PointToPlaneIntersection(common_msgs::Point3d p, const double &a, const double &b, const double &c, const double &d, common_msgs::Point3d &plane_p0)
{
// A 为平面的法向量 B为平面上的一点目标点形成的向量
// A . B = |A||B|cos(angle)
// 点到面的高度为 (A.B) / |A|
// common_msgs::Point3d plane_p0;
plane_p0.x = 0;
plane_p0.y = 0.0;
plane_p0.z = (-d - a * plane_p0.x - b * plane_p0.y) / c;
// 两个向量点乘的结果
double numerator = std::abs((p.x - plane_p0.x) * a + (p.y - plane_p0.y) * b + (p.z - plane_p0.z) * c);
// 法向量的模
double denominator = std::sqrt(a * a + b * b + c * c);
//点到面的距离
double hight = numerator / denominator;
//这个点在法向量上 与法向量成一定的比例关系
common_msgs::Point3d intersection;
intersection.x = p.x - (hight / denominator) * a;
intersection.y = p.y - (hight / denominator) * b;
intersection.z = p.z - (hight / denominator) * c;
return intersection;
}