两条空间直线求交点

设空间曲线为：
$$\frac{x-b_1}{a_1}=\frac{y-d_1}{c_1}=\frac{z}{1}$$
$$\frac{x-b_2}{a_2}=\frac{y-d_2}{c_2}=\frac{z}{1}$$
当两条直线存在交点时，交点$(x_0,y_0,z_0)$同时满足两条曲线方程，此时：
$$x_0=a_1{z}_0+b_1=a_2{z}_0+b_2$$
$$y_0=c_1{z}_0+d_1=c_2{z}_0+d_2$$
联立两式可得：
$$z_0=\frac{b_2-b_1+d_1-d_2}{a_1-a_2+c_2-c_1}$$
$$\{\begin{array}{l}{x}_0=a_1\times \frac{b_2-b_1+d_1-d_2}{a_1-a_2+c_2-c_1}+b_1\\ 或\\ x_0=a_2\times \frac{b_2-b_1+d_1-d_2}{a_1-a_2+c_2-c_1}+b_2\end{array}$$
$$\{\begin{array}{l}{y}_0=c_1\times \frac{b_2-b_1+d_1-d_2}{a_1-a_2+c_2-c_1}+d_1\\ 或\\ y_0=c_2\times \frac{b_2-b_1+d_1-d_2}{a_1-a_2+c_2-c_1}+d_2\end{array}$$

C++程序为：

#include<iostream>
#include<math.h>

/*
设空间直线1方程为：
x+2   y-1    z
--- = --- = ---
 1     2     1
 
a1 = e[0][0] = 1
b1 = e[0][1] = 2
c1 = e[1][0] = 2
d1 = e[1][1] = 1
 
空间直线2方程为：
x+4   y-2    z
--- = --- = ---
 3     1     1

a2 = e1[0][0] = 3
b2 = e1[0][1] = 4
c2 = e1[1][0] = 1
d2 = e1[1][1] = 2

*/ 

using namespace std;
void matrix_inverse(double(*a)[2], double(*b)[2]);

int main()
{
	// 直线1 
	int i, j, M, N;
	double f[][3] = {
						{ -2, 1, 0 },
						{ -1, 3, 1 },
						{  0, 5, 2 }
					};
	M = sizeof(f) / sizeof(f[0]);
	
	double a[M][3] = { 0 };
	for ( i = 0; i < M; i++ )
		for( j = 0; j < 3; j++ )
			a[i][j] = f[i][j];
	
	double b[2][2] = { 0 };
	double c[2][2] = { 0 };
	double d[2][2] = { 0 };
	double e[2][2] = { 0 };
	
	for ( i = 0; i < M; i++ )
	{
		b[0][0] = b[0][0] + ( a[i][0] * a[i][2] );
		b[0][1] = b[0][1] + a[i][0];
		b[1][0] = b[1][0] + ( a[i][1] * a[i][2] );
		b[1][1] = b[1][1] + a[i][1];
		c[0][0] = c[0][0] + pow( a[i][2], 2 );
		c[0][1] = c[0][1] + a[i][2];
		c[1][0] = c[1][0] + a[i][2];
		c[1][1] = M;
	}
	
	matrix_inverse(c, d);  //求逆函数
	
	e[0][0] = (b[0][0] * d[0][0]) + (b[0][1] * d[1][0]);	// a
	e[0][1] = (b[0][0] * d[0][1]) + (b[0][1] * d[1][1]);	// b
	e[1][0] = (b[1][0] * d[0][0]) + (b[1][1] * d[1][0]);	// c
	e[1][1] = (b[1][0] * d[0][1]) + (b[1][1] * d[1][1]);	// d

	// 直线2 
	int i1, j1, M1, N1;
	double f1[][3] = {
						{ -4, 2, 0 },
						{ -1, 3, 1 },
						{  2, 4, 2 }
					};
	M1 = sizeof(f1) / sizeof(f1[0]);
	
	double a1[M1][3] = { 0 };
	for ( i1 = 0; i1 < M1; i1++ )
		for( j1 = 0; j1 < 3; j1++ )
			a1[i1][j1] = f1[i1][j1];
	
	double b1[2][2] = { 0 };
	double c1[2][2] = { 0 };
	double d1[2][2] = { 0 };
	double e1[2][2] = { 0 };
	
	for ( i1 = 0; i1 < M1; i1++ )
	{
		b1[0][0] = b1[0][0] + ( a1[i1][0] * a1[i1][2] );
		b1[0][1] = b1[0][1] + a1[i1][0];
		b1[1][0] = b1[1][0] + ( a1[i1][1] * a1[i1][2] );
		b1[1][1] = b1[1][1] + a1[i1][1];
		c1[0][0] = c1[0][0] + pow( a1[i1][2], 2 );
		c1[0][1] = c1[0][1] + a1[i1][2];
		c1[1][0] = c1[1][0] + a1[i1][2];
		c1[1][1] = M1;
	}
	
	matrix_inverse(c1, d1);  //求逆函数
	
	e1[0][0] = (b1[0][0] * d1[0][0]) + (b1[0][1] * d1[1][0]);	// a1
	e1[0][1] = (b1[0][0] * d1[0][1]) + (b1[0][1] * d1[1][1]);	// b1
	e1[1][0] = (b1[1][0] * d1[0][0]) + (b1[1][1] * d1[1][0]);	// c1
	e1[1][1] = (b1[1][0] * d1[0][1]) + (b1[1][1] * d1[1][1]);	// d1

	double x0,y0,z0;	// 计算对称度时仅需计算出z0即可 
	z0 = ((e1[0][1] - e1[1][1])-(e[0][1] - e[1][1]))/((e[0][0]-e[1][0])-(e1[0][0]-e1[1][0]));
	x0 = ( e1[0][0] * z0 ) + e1[0][1];
	y0 = ( e1[1][0] * z0 ) + e1[1][1];
	
//	cout << " " << x0 << "\t";
//	cout << " " << y0 << "\t";
//	cout << " " << z0 << "\t";
	return 0;
 }
 
 void matrix_inverse(double(*a)[2], double(*b)[2])
{
	using namespace std;
	int i, j, k;
	double max, temp;
	// 定义一个临时矩阵t
	double t[2][2];
	// 将a矩阵临时存放在矩阵t[n][n]中
	for (i = 0; i < 2; i++)
		for (j = 0; j < 2; j++)
			t[i][j] = a[i][j];
	// 初始化B矩阵为单位矩阵
	for (i = 0; i < 2; i++)
	{
		for (j = 0; j < 2; j++)
		{
			b[i][j] = (i == j) ? (double)1 : 0;
		}
	}
	// 进行列主消元，找到每一列的主元
	for (i = 0; i < 2; i++)
	{
		max = t[i][i];
		// 用于记录每一列中的第几个元素为主元
		k = i;
		// 寻找每一列中的主元元素
		for (j = i + 1; j < 2; j++)
		{
			if (fabs(t[j][i]) > fabs(max))
			{
				max = t[j][i];
				k = j;
			}
		}
		//cout<<"the max number is "<<max<<endl;
		// 如果主元所在的行不是第i行，则进行行交换
		if (k != i)
		{
			// 进行行交换
			for (j = 0; j < 2; j++)
			{
				temp = t[i][j];
				t[i][j] = t[k][j];
				t[k][j] = temp;
				// 伴随矩阵B也要进行行交换
				temp = b[i][j];
				b[i][j] = b[k][j];
				b[k][j] = temp;
			}
		}
		if (t[i][i] == 0)
		{
			cout << "\nthe matrix does not exist inverse matrix\n";
			break;
		}
		// 获取列主元素
		temp = t[i][i];
		// 将主元所在的行进行单位化处理
		//cout<<"\nthe temp is "<<temp<<endl;
		for (j = 0; j < 2; j++)
		{
			t[i][j] = t[i][j] / temp;
			b[i][j] = b[i][j] / temp;
		}
		for (j = 0; j < 2; j++)
		{
			if (j != i)
			{
				temp = t[j][i];
				//消去该列的其他元素
				for (k = 0; k < 2; k++)
				{
					t[j][k] = t[j][k] - temp * t[i][k];
					b[j][k] = b[j][k] - temp * b[i][k];
				}
			}
		}
	}
}