距離の公式
次の平面と点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との距離
この状態では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}$$
として計算できます。
実際にOpenCVで計算すると次のようなコードになります。ノルムはベクトルの長さを計算しますが、Mat型しか引数に取れないためキャストします。
#include "opencv/cv.h"
#include "opencv/highgui.h"
using namespace cv;
double dist(Point3f a, Point3f b, Point3f c, Point3f p){
Vec3f PA = p-a;
Vec3f PB = p-b;
Vec3f PC = p-c;
double V = norm(Mat(PA.cross(PB.dot(PC))))/6.0;
Vec3f AB = b-a;
Vec3f AC = c-a;
double S = norm(Mat(AB.cross(AC)))/2.0;
return 3.0*V/S;
}
int main(){
Point3f a = Point3f(4.0, 0.0, 2.0);
Point3f b = Point3f(1.0, 5.0, 1.0);
Point3f c = Point3f(6.0, 2.0, 3.0);
Point3f p = Point3f(4.0, 3.0, 9.0);
printf("%f\n",dist(a,b,c,p));
}
「法線ベクトルと平面上の1点」で表された平面と点Pとの距離
法線ベクトルはあらかじめ単位ベクトル化されているものとし、nとして表記することにします。この場合はPと平面との距離を計算するのはとても簡単で、ベクトルAPを計算し、APとnの内積を計算して絶対値をとるだけです。APのnへの射影は平面に垂直で、かつ、Pを端点にもつベクトルになるため、このベクトルの大きさでPから平面への距離を計算できます。絶対値を取るのは平面に対してベクトルnと点Pが逆を向いている場合に対応するためです。
$$D=|\vec{n}\cdot AP| ただし |\vec{n}|=1$$
OpenCVを用いてベクトルを単位ベクトル化するコードを先に紹介しておきます。ノルムはベクトルの長さを計算しますが、Mat型しか引数に取れないためキャストします。大きさを平方根で求めてベクトルを割ることで単位ベクトルを計算します。
Vec3f Normarize(Vec3f v){
return v/norm(Mat(v));
}
最終的な実装が下のコードです。
#include "opencv/cv.h"
#include "opencv/highgui.h"
#include <math.h>
using namespace cv;
using namespace std;
Vec3f Normarize(Vec3f v){
return v/norm(Mat(v));
}
double dist(Point3f P, Point3f A, Vec3f N){
Vec3f AP = (P-A);
return fabs(Normarize(N).dot(AP));
}
int main(){
Point3f P = Point3f(1.0, 3.0, 1.5);
Point3f A = Point3f(4.0, -1.0, 4.0);
Vec3f N = Vec3f(1.0, 1.0, -2.5);
printf("%f\n",dist(P,A,N));
}