最近在看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; }
算法优点:
矢量法代码简单,计算量少。无需进行复杂的分类讨论,无需进行角度计算,无需进行面积计算等。