点到线段的最短距离——矢量法

最近在看recast&detour源码的时候有遇到许多数学上的算法问题，特此记录，以便以后查看。

矢量法推导： 

求点P到线段AB的最短距离。分成以下三种情况(a),(b),(c)。

（勘误：d=PC 应该是在 ∠PAB和∠PBA都小于90°的情况下，而不是▲ABP为锐角▲）

所以可以先根据计算出r的值，进而对应计算A点 B点  C点 和 P点之间的距离即可。

特殊情况：

1.当P在线段AB上：计算出来r仍然是 1>r>0, P点即C点，PC的距离d = 0； 

2.当P在线段AB端点或其延长线上：r仍然有是 r<=0 或者是 r >= 1，仍然是计算 PA 或 PB 的距离；

3.当AB是同一点：无法计算r，所以需要对AB的长度进行一个判断，如果是AB为零，直接令r = 0，直接计算AP或BP的距离都一样。

库中代码：

点pt 到线段pq的最短距离。

static float distancePtSeg(const float* pt, const float* p, const float* q)
{

    // 先计算r的值 看r的范围 （p相当于A点，q相当于B点，pt相当于P点）
    // AB 向量
	float pqx = q[0] - p[0];
	float pqy = q[1] - p[1];
	float pqz = q[2] - p[2];
    // AP 向量
	float dx = pt[0] - p[0];
	float dy = pt[1] - p[1];
	float dz = pt[2] - p[2];

    // qp线段长度的平方=上面公式中的分母：AB向量的平方。
	float d = pqx*pqx + pqy*pqy + pqz*pqz;   
    // （p pt向量）点积 （pq 向量）= 公式中的分子：AP点积AB
	float t = pqx*dx + pqy*dy + pqz*dz;         

    // t 就是 公式中的r了 
	if (d > 0)         // 除数不能为0; 如果为零 t应该也为零。下面计算结果仍然成立。                   
		t /= d;    // 此时t 相当于 上述推导中的 r。
	
    // 分类讨论 
    if (t < 0)
		t = 0;     // 当t（r）< 0时，最短距离即为 pt点 和 p点（A点和P点）之间的距离。
	else if (t > 1)
		t = 1;     // 当t（r）> 1时，最短距离即为 pt点 和 q点（B点和P点）之间的距离。

	// t = 0，计算 pt点 和 p点的距离; （A点和P点）
    // t = 1, 计算 pt点 和 q点 的距离; （B点和P点）
    // 否则计算 pt点 和 投影点 的距离。（C点和P点 ，t*（pqx，pqy，pqz）就是向量AC）
	dx = p[0] + t*pqx - pt[0];
	dy = p[1] + t*pqy - pt[1];
	dz = p[2] + t*pqz - pt[2];

    // 算出来是距离的平方，后续自行计算距离
	return dx*dx + dy*dy + dz*dz;
}

算法优点：

矢量法代码简单，计算量少。无需进行复杂的分类讨论，无需进行角度计算，无需进行面积计算等。

参考：

新浪博客

GitHub - recastnavigation/recastnavigation: Navigation-mesh Toolset for Games