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

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


矢量法推导: 

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


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

特殊情况:

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

当P在线段AB端点或其延长线上:r仍然有是 r<=0 或者是 r >= 1,仍然是计算 PA 或 PB 的距离;

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


库中代码:

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

static float distancePtSeg(const float* pt, const float* p, const float* q)
{
	float pqx = q[0] - p[0];
	float pqy = q[1] - p[1];
	float pqz = q[2] - p[2];
	float dx = pt[0] - p[0];
	float dy = pt[1] - p[1];
	float dz = pt[2] - p[2];
	float d = pqx*pqx + pqy*pqy + pqz*pqz;      // qp线段长度的平方
	float t = pqx*dx + pqy*dy + pqz*dz;         // p pt向量 点积 pq 向量(p相当于A点,q相当于B点,pt相当于P点)
	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点的距离; t = 1, 计算 pt点 和 q点 的距离; 否则计算 pt点 和 投影点 的距离。
	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;
}

算法优点:

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


参考:

http://blog.sina.com.cn/s/blog_5d5c80840101bnhw.html

https://github.com/recastnavigation/recastnavigation

猜你喜欢

转载自blog.csdn.net/u012138730/article/details/79779996