點到線段的最短距離——向量法
阿新 • • 發佈:2018-12-24
最近在看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是同一點:
庫中程式碼:
點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; }
演算法優點:
向量法程式碼簡單,計算量少。無需進行復雜的分類討論,無需進行角度計算,無需進行面積計算等。