線分と平面の交点を求める

線分の両端点(a, b)と平面の法線ベクトル(nv)と平面上の任意の点(p)から、線分と平面の交点を求めるC++のプログラム。

線分abと平面の交点が線分abを内分する点となることから交点の座標を計算する。

#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <string>

using namespace std;

cv::Mat Intersect3D(
		 cv::Mat& nv, // 平面の法線ベクトル cv::Mat(3,1)
		 cv::Mat& p,  // 平面上の任意の1点  cv::Mat(3,1)
		 cv::Mat& a,  // 線分の端点         cv::Mat(3,1)
		 cv::Mat& b   // 線分の端点         cv::Mat(3,1)
		 ){
  
  // 返り値
  cv::Mat ret = (cv::Mat_<double>(3,1) << 0,0,0);

  // ベクトルの準備
  cv::Mat pa, pb; // p->a, p->bのベクトル
  pa = a - p;
  pb = b - p;

  // 内積計算
  double dot_a = pa.dot(nv); // paベクトルと法線ベクトルの内積
  double dot_b = pb.dot(nv); // pbベクトルと法線ベクトルの内積
 
  // 内積が0の場合交点がなく、直線が平面に含まれる
  double MIN_DOT_TH = 0.000001;
  if( fabs(dot_a) < MIN_DOT_TH ){dot_a = 0;}
  if( fabs(dot_b) < MIN_DOT_TH ){dot_b = 0;}
  
  // 交点なし
  if( dot_a==0 && dot_b==0){
    return ret; // [0,0,0]'で返す
  }

  // 交点なし
  if( dot_a*dot_b > 0 ){
    return ret; // [0,0,0]'で返す
  }

  // 交点あり
  cv::Mat ab = b - a;
  double ratio = fabs(dot_a) / (fabs(dot_a) + fabs(dot_b) );
  ret = a + ab * (ratio);

  return ret; // 計算結果を返す
}



int main(int argc, char *argv[]){

  cv::Mat a  = (cv::Mat_<double>(3,1) << 0,0, 0); // 端点1
  cv::Mat b  = (cv::Mat_<double>(3,1) << 2,2,-2); // 端点2
  cv::Mat p  = (cv::Mat_<double>(3,1) << 0,0,-1); // 平面上の点
  cv::Mat nv = (cv::Mat_<double>(3,1) << 0,0, 1); // 法線ベクトル
  cv::Mat c = Intersect3D(nv, p, a, b); // 交点

  cout << "result: " << c << endl;
  
  return 0;
}