直线与平面的交点

xxx.c

/**
 * @brief 计算直线与平面的交点
 * @param[in] planeVector - 平面上向量【目前为[0, 0, 100]】
 * @param[in] planePoint - 平面上一点
 * @param[in] lineVector - 直线向量
 * @param[in] linePoint - 直线上一点
 * @param[out] resultPoint- 直线与平面的交点【失败resultPoint值全为 -1】
 * @return 无
 * @note 调用约定:
 * 1. 数组大小均为3*sizeof(float)
 */
static void CalPlaneLineIntersectPoint(float planeVector[], float planePoint[],
                                       float lineVector[], float linePoint[],
                                       float resultPoint[]);


void CalPlaneLineIntersectPoint(float planeVector[], float planePoint[],

                                float lineVector[], float linePoint[],

                                float resultPoint[])

{

    float vp1, vp2, vp3, n1, n2, n3, v1, v2, v3, m1, m2, m3, t,vpt;

    vp1 = planeVector[0];

    vp2 = planeVector[1];

    vp3 = planeVector[2];

    n1 = planePoint[0];

    n2 = planePoint[1];

    n3 = planePoint[2];

    v1 = lineVector[0];

    v2 = lineVector[1];

    v3 = lineVector[2];

    m1 = linePoint[0];

    m2 = linePoint[1];

    m3 = linePoint[2];

    vpt = v1 * vp1 + v2 * vp2 + v3 * vp3;

    //首先判断直线是否与平面平行

    if (vpt == 0)

    {

        resultPoint[0] = -1;

        resultPoint[1] = -1;

        resultPoint[2] = -1;

    }

    else

    {

        t = ((n1 - m1) * vp1 + (n2 - m2) * vp2 + (n3 - m3) * vp3) / vpt;

        resultPoint[0] = m1 + v1 * t;

        resultPoint[1] = m2 + v2 * t;

        resultPoint[2] = m3 + v3 * t;

    }

    return;

}