ベクトルA： ( x , y , z )=(−1,2,0)+(2,3,1)p…(A)
ベクトルB： ( x , y , z )=(3,−4,1)+(1,2,1)q…(B)

に関し、２つのベクトルが最短距離になる位置での「p」「q」を求めよ。

#include <iostream>
#include <vector>
#include <numeric>
#include <cv.h>
#include <cxcore.h>
#include <boost/assign.hpp>

struct Ray
{
	std::vector<double> Start; //スタート位置
	std::vector<double> vec; //方向ベクトル
};

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

	Ray First;
	Ray Second;
	{
		using namespace boost::assign;
		First.Start += -1,2,0;
		First.vec += 2,3,1;
		Second.Start += 3,-4,1;
		Second.vec += 1,2,1;
	}

	
	double vecAB[4]={
		std::inner_product(First.vec.begin(),First.vec.end(),First.vec.begin(),0.0),
		-std::inner_product(Second.vec.begin(),Second.vec.end(),First.vec.begin(),0.0),
		std::inner_product(First.vec.begin(),First.vec.end(),Second.vec.begin(),0.0),
		-std::inner_product(Second.vec.begin(),Second.vec.end(),Second.vec.begin(),0.0)
	};

	std::vector<double> AB(3);
	AB[0] = First.Start[0] - Second.Start[0];
	AB[1] = First.Start[1] - Second.Start[1];
	AB[2] = First.Start[2] - Second.Start[2];
	
	
	double Ydata[2] ={
		-std::inner_product(AB.begin(),AB.end(),First.vec.begin(),0.0),
		-std::inner_product(AB.begin(),AB.end(),Second.vec.begin(),0.0)
	};
	
	//行列を作成
	CvMat MatA = cvMat(2,2,CV_64FC1,vecAB);
	CvMat MatY = cvMat(2,1,CV_64FC1,Ydata);
	CvMat *MatX = cvCreateMat(2,1,CV_64FC1);
	cvSolve( &MatA, &MatY, MatX, CV_LU ); //方程式を解く

	std::cout <<"p:"<< MatX->data.db[0] << "\n";
	std::cout <<"q:"<<MatX->data.db[1] << "\n";

	cvReleaseMat(&MatX);

	return EXIT_SUCCESS;
}