点と平面の距離(三重積と法線)

距離の公式

次の平面と点Pが与えられたとき、

ax+by+cz+n=0 \\
P (x_{0},y_{0},z_{0})

平面との距離Dは以下の式により求められます。

D=\frac{|ax_{0}+by_{0}+cz_{0}+n|}{\sqrt{ a^2+b^2+c^2 }}

二次元においては、c=0をそれぞれ代入すると、

次の直線と点Qが与えられたとき、

ax+by+n=0 \\
Q (x_{0},y_{0})

直線との距離Dは以下の式により求められます。

D=\frac{|ax_{0}+by_{0}+n|}{\sqrt{ a^2+b^2 }}

プログラミングでよく用いられる平面の表現方法

ここまでは一般的なものですが、三次元空間上で平面を一意に指定するために、「平面上の3点」あるいは「法線ベクトルと平面上の1点」どちらかを指定する、とうことがよく行われます。「平面上の3点」はその3点の順番によって、「法線ベクトルと平面上の1点」はその法線ベクトルの向きによって、どちらも平面の裏表を表現することもできます。

ここでは、この2つのパターンで表現された平面と任意の点Pとの距離を計算する関数を作ってみることにします。

「平面上の3点」で表された平面と点Pとの距離

file

この状態では4点が指定されていることになります。すると、これら4点で構成される四面体が存在します。指定された平面上の3点をA,B,Cとして、ベクトルPA,PB,PCを計算します。3つのベクトルPA(x1,y1,z1),PB(x2,y2,z2),PC(x3,y3,z3)に囲まれる四面体PABCの体積Vは、スカラー三重積の1/6で計算できます。

V=\frac{1}{6}\vec{PA}\times(\vec{PB}\cdot\vec{PC})

ただし、実際にコード上で計算する場合には、この展開形の次式を使っても良いでしょう。

V=\frac{1}{6}|y_{1}z_{2}x_{3}+z_{1}x_{2}y_{3}+x_{1}y_{2}z_{3}-z_{1}y_{2}x_{3}-x_{1}z_{2}y_{3}-y_{1}x_{2}z_{3}|

続いて、ABCの面積を求めます。先と同様にベクトルAB,ACをまず計算します。ベクトルの外積の大きさは外積計算に用いた2つのベクトルを2辺にもつ平行四辺形の面積に等しいので、2つのベクトルAB,ACに囲まれる三角形の面積Sは外積の大きさの1/2となることから、

S=\frac{1}{2}|\vec{AB}\times\vec{AC}|

として計算できます。

ところで、四面体は三角錐でもあるので、体積は底面積×高さでも計算できるはずです。底面積をS、高さをhとすると、体積Vとの関係は次のようになります。

V=\frac{Sh}{3}

実はこの高さこそが求めたい平面ABCとPとの距離になります。したがって距離Dは

D=h=\frac{3V}{S}

として計算できます。

実際にPythonで計算すると次のようなコードになります。np.linalg.normはベクトルの長さを計算します。

import numpy as np

def dist(a,b,c,p):
    PA = p-a
    PB = p-b
    PC = p-c
    V = np.linalg.norm(np.cross(PA,PB.dot(PC)))/6.0

    AB = b-a
    AC = c-a
    S = np.linalg.norm(np.cross(AB,AC))/2.0

    return 3.0*V/S

「法線ベクトルと平面上の1点」で表された平面と点Pとの距離

file

法線ベクトルはあらかじめ単位ベクトル化されているものとし、nとして表記することにします。この場合はPと平面との距離を計算するのはとても簡単で、ベクトルAPを計算し、APとnの内積を計算して絶対値をとるだけです。APのnへの射影は平面に垂直で、かつ、Pを端点にもつベクトルになるため、このベクトルの大きさでPから平面への距離を計算できます。絶対値を取るのは平面に対してベクトルnと点Pが逆を向いている場合に対応するためです。

D=|\vec{n}\cdot AP|  ただし |\vec{n}|=1

normalizeは、ベクトルを単位ベクトル化する関数です。

import numpy as np

def dist(a,n,p):
    AP = p-a
    return np.abs(np.dot(normalize(n),AP))

def normalize(v):
    norm = np.linalg.norm(v)
    if norm == 0:
        return v
    return v / norm

a = np.array([1,2,3])
n = np.array([1,1,1])
p = np.array([1,1,1])

print(dist(a,n,p))