distance geometry

点と直線の距離

点と直線の距離に関するアルゴリズムをまとめます.
Feb. 2, 2020, 1:51 p.m.

目次

直線、線分の定義

直線はある2点を通る線であり、無限に続きます.
一方で線分はある2点を結ぶ線であり、その2点が終端となります.

点と点の距離

解説

点と点の距離はx座標とy座標それぞれの距離をもとめて三平方の定理でもとまります.
結局2乗するのでどちらの点から引いても良いです.

$$
\sqrt{(x_2 - x_1)^2 + (y_2 - y_1)^2}
$$

共通のVectorクラスを使うと以下のようになります.

コード

1
2
3
double p2p(Vector p1, Vector p2) {
  return (p2 - p1).length();
}

点と直線の距離

解説

直線は無限に続くので、1点からは無数の長さが取れますがその中で最短のものが距離です.
点から直線に対して垂直に線を垂らして出来る線分の長さがその距離になります.

点A, Bを通る直線をAB、点をPとすると距離は、ABPが作る平行四辺形の高さと考えることが出来ます.
平行四辺形の面積はベクトルABとベクトルAPの外積の絶対値で、かつ底辺の長さはABの長さ.
よって、高さはベクトルABとベクトルAPの外積の絶対値 / ABの長さとなります.

$$
\frac{|\vec{AB} \times \vec{AP}|}{\|\vec{AB}\|}
$$

コード

1
2
3
double p2Line(Vector2 a, Vector2 b, Vector2 p) {
  return abs((b - a).cross(p - a)) / (b - a).length();
}

点と線分の距離

解説

点A, Bを結ぶ線分と点Pを考えます.
線分ABは直線のように無限に続くわけでは無いので、必ずしも点Pから垂線を垂らして線分にぶつかるとは限りません.
垂線PHを直線ABに垂らした時に点Hが線分ABとどういう位置関係にあるかで3通りの場合分けが必要です.

  1. HがAの外にある場合 → PとAの距離が最短です
  2. HがBの外にある場合 → PとBの距離が最短です
  3. HがAB上にある場合 → PHが最短です(点と直線の距離)

HがAの外側にあるかどうかは$\angle{PAB}$の角度が$90^\circ$より大きいか調べれば分かります.
ベクトルABとベクトルAPの内積が負であれば$90^\circ$よりも大きいということになります.
HがBの外側にあることは、ベクトルを反転させて同じ手順で分かります.

コード

1
2
3
4
5
double p2Seg(Vector2 a, Vector2 b, Vector2 p) {
  if ((p - a).dot(b - a) < 0.0) return (p - a).length();
  if ((p - b).dot(a - b) < 0.0) return (p - b).length();
  return p2Line(a, b, p);
}

線分と線分の距離

解説

線分ABと線分CDの距離は、線分同士が交わる場合には0となる.
その他の場合は以下4通りの最短が距離となる.

  1. 点Aと線分CDの距離
  2. 点Bと線分CDの距離
  3. 点Cと線分ABの距離
  4. 点Dと線分ABの距離

点と線分の距離の求め方は前述の通り.

交差するかどうかは線分ABを直線の方程式に直し、その方程式に点CとDを代入して直線の左側と右側に分かれていたら交差すると判定できる.
$y < ax + b$ なら左側.
$y > ax + b$ なら右側.

事前に式変形をしておくと以下ソースコードの通りになる.

コード

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
double distance(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4) {
  double t3 = (p1.x - p2.x) * (p3.y - p1.y) + (p1.y - p2.y) * (p1.x - p3.x);
  double t4 = (p1.x - p2.x) * (p4.y - p1.y) + (p1.y - p2.y) * (p1.x - p4.x);
  double t1 = (p3.x - p4.x) * (p1.y - p3.y) + (p3.y - p4.y) * (p3.x - p1.x);
  double t2 = (p3.x - p4.x) * (p2.y - p3.y) + (p3.y - p4.y) * (p3.x - p2.x);
  if (t3 * t4 < 0 && t1 * t2 < 0) return 0.0;
  double min_ = -1;
  min_ = min<double>(p2Seg(p1, p2, p3), p2Seg(p1, p2, p4));
  min_ = min<double>(min_, p2Seg(p3, p4, p1));
  min_ = min<double>(min_, p2Seg(p3, p4, p2));
  return min_;
}

別解

交差判定のロジックを、位置ベクトルが時計回りかどうか判定する方法を用いてもっと分かりやすく書くことも出来る.

別解コード

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
int cw(Vector2 v01, Vector2 v02) {
  double cross_ = v01.cross(v02);
  if (cross_ > EPS) return 1; // above segment
  else if (cross_ < -EPS) return -1; // beneath segment
  if (v01.dot(v02) < 0) return 2; // opposite
  if (v01.length() - v02.length() >= 0) return 0; // on segment
  return -2; // out of segment
}

double distance(Vector2 p1, Vector2 p2, Vector2 p3, Vector2 p4) {
  int cw1 = cw(p2 - p1, p3 - p1) * cw(p2 - p1, p4 - p1);
  int cw2 = cw(p4 - p3, p1 - p3) * cw(p4 - p3, p2 - p3);
  if (cw1 <= 0 && cw2 <= 0) return 0.0;
  double min_ = -1;
  min_ = min<double>(p2Seg(p1, p2, p3), p2Seg(p1, p2, p4));
  min_ = min<double>(min_, p2Seg(p3, p4, p1));
  min_ = min<double>(min_, p2Seg(p3, p4, p2));
  return min_;
}