点在平面上的投影坐标

点在平面上的投影坐标

原理

通过平面上随意找一个点与之形成向量,通过向量与平面的法向量点乘得到距离值

代码


    /**
     * @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;
    }

效果显示

在这里插入图片描述

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值