点到线段的最小距离（C++）

/*********************************/
// 如果经过点做直线的垂足，垂足落在线段上，则取垂线段的距离
// 否则取到线段两端点距离的最小值
//
// 参数：
// point:  存储点的xy坐标
// p1, p2: 线段的两点
//
// return: 点到线段的最小距离
/*********************************/

float CalculatePointToLineDistance(float point[2], const float p1[2], const float p2[2])
{
    float dis = 0.f;

    float dx = p2[0] - p1[0];
    float dy = p2[1] - p1[1];

    // 两直线垂直，向量表示法，转换后公示
    float k = -((p1[0] - point[0])*dx + (p1[1] - point[1])*dy) / ( dx*dx + dy*dy);
    float footX = k*dx + p1[0];
    float footY = k*dy + p1[1];

    //if垂足是否落在线段上
    if( footY >= min(p1[1], p2[1]) && footY <=max(p1[1], p2[1])
        && footX >= min(p1[0], p2[0]) && footX <=max(p1[0], p2[0] ) )
    {
        dis = sqrtf((footX-point[0])*(footX-point[0]) + (footY-point[1])*(footY-point[1]));
    }
    else 
    {
        float dis1 = sqrtf((p1[0]-point[0])*(p1[0]-point[0]) + (p1[1]-point[1])*(p1[1]-point[1]));
        float dis2 = sqrtf((p2[0]-point[0])*(p2[0]-point[0]) + (p2[1]-point[1])*(p2[1]-point[1]));

        dis = ( dis1 < dis2 ? dis1 : dis2 );
    }

    return dis;
}