有一个参考的博客,大家搜一下就可以找到,这是我用到的用eigen库的
//pt 空间外一点
//begin-end 直线上的两个点
Eigen::Vector3d GetFootOfPerpendicular(const Eigen::Vector3d &pt, const Eigen::Vector3d &begin, const Eigen::Vector3d &end)
//pt:point outside line
{
auto diff = end - begin;
auto diff_sqr_norm = diff.squaredNorm();
if (diff_sqr_norm < 0.00000001)
{
return begin;
}
double u = ((pt - begin).transpose() * diff)(0) / diff_sqr_norm;;
return begin + u * diff;
}