求空间点沿平面法向量与平面交点的坐标

陆先森不怕鬼

已于 2023-03-24 16:20:05 修改

阅读量853

点赞数

文章标签：平面算法

于 2023-03-06 10:32:58 首次发布

本文链接：https://blog.csdn.net/weixin_44729155/article/details/129356379

版权

假定平面方程为: $a x + b y - z + d = 0$ ，空间点坐标为 $x_0,y_0,z_0)$ 。
由于直线过点 $x_0,y_0,z_0)$ ，方向向量为 $(a, b, - 1)$ ，所以直线方程可写为：
$\frac{x-x_0}{a}=\frac{y-y_0}{b}=z_0-z$
将直线方程代入空间方程后可得
$a[(z_0-z)a+x_0]+b[(z_0-z)b+y_0]-z+d=0$
$z=\frac{a(az_0+x_0)+b(bz_0+y_0)+d}{a^2+b^2+1}$
将z的值代入直线方程便可获得交点的x与y坐标。

C++程序为：

#include<iostream>
#include <iomanip>
#include<math.h>
#define PAI acos(-1)

using namespace std;

double point[][3] = {{-3,0,5},{0,3,5},{0,-3,5},{3,0,5}};		// 圆心坐标(0, 0, 5) 
double point1[][3] = {{-1,0,1},{0,1,1},{0,-1,1},{1,0,1}};	// 圆心坐标(0, 0, 1) 
int size = sizeof(point) / sizeof(point[0]);
int size1 = sizeof(point1) / sizeof(point1[0]);

void matrix_inverse(double(*a)[3], double(*b)[3]);
void Fit_Circle(double (*circle_point)[3]);

int main() 
{
	double RR[3][3] = {0};	// R_T * R
	double RR_1[3][3] = {0};
	double A[3][1] = {0};

	int i, j, k;
	double **R = new double*[size1];
	for (i = 0; i < size1; i++)
		R[i] = new double[3];

	double **R_T = new double*[3];
	for (i = 0; i < 3; i++)
		R_T[i] = new double[size1];

	double **RR_1R_T = new double*[3];
	for (i = 0; i < 3; i++)
		RR_1R_T[i] = new double[size1];

	double **Z = new double*[size1];
	for (i = 0; i < size1; i++)
		Z[i] = new double[1];
		
	for(i = 0; i < size1; i++)
	{
		R[i][0] = point1[i][0];
		R[i][1] = point1[i][1];
		R[i][2] = 1;
		
		R_T[0][i] = point1[i][0];
		R_T[1][i] = point1[i][1];
		R_T[2][i] = 1;
		
		Z[i][0] = point1[i][2];
	}
	
	for(i = 0; i < 3; ++i)		// 矩阵相乘 (k的取值来源于左边矩阵的列数或右边矩阵的行数） 
	{
		for(j = 0; j < 3; ++j)
		{
			RR[i][j] = 0;
			for(k = 0; k < size1; ++k)
				RR[i][j] += R_T[i][k] * R[k][j];
		}
	}

	matrix_inverse(RR, RR_1);
	
	for(i = 0; i < 3; ++i)
	{
		for(j = 0; j < size1; ++j)
		{
			RR_1R_T[i][j] = 0; 
			for(k = 0; k < 3; ++k)
				RR_1R_T[i][j] += RR_1[i][k] * R_T[k][j];
		}
	}

	for(i = 0; i < 3; ++i)
	{
		for(j = 0; j < 1; ++j)
		{
			A[i][j] = 0;
			for(k = 0; k < size1; ++k)
				A[i][j] += RR_1R_T[i][k] * Z[k][j];
		}
	}
	
//	double Theta_XOY = asin(fabs(-1) / (sqrt(pow(A[0][0], 2) + pow(A[1][0], 2) + 1))) * (180 / PAI) ;
//	double Theta_YOZ = asin(fabs(A[0][0]) / (sqrt(pow(A[0][0], 2) + pow(A[1][0], 2) + 1))) * (180 / PAI) ;
//	double Theta_XOZ = asin(fabs(A[1][0]) / (sqrt(pow(A[0][0], 2) + pow(A[1][0], 2) + 1))) * (180 / PAI) ;
	
//	cout.setf(ios::fixed);
//	cout << "平面方程为：z=" << setprecision(2) << A[0][0] << "*x+(" << setprecision(2) << A[1][0] << ")*y+(" << setprecision(2) << A[2][0] << ")" << endl;
//	cout << "法向量为：(" << setprecision(2) << A[0][0] << "," << setprecision(2) << A[1][0] << ", -1)" << endl;
//	
//	cout << "平面法向量与XOY平面夹角为：" << setprecision(3) << Theta_XOY << endl;
//	cout << "平面法向量与YOZ平面夹角为：" << setprecision(3) << Theta_YOZ << endl;
//	cout << "平面法向量与XOZ平面夹角为：" << setprecision(3) << Theta_XOZ << endl;

	double circle[1][3] = {0};
	Fit_Circle(circle);
	
	double t,r,z;
	z = (A[0][0]*(A[0][0]*circle[0][2]+circle[0][0])+A[0][1]*(A[0][1]*circle[0][2]+circle[0][1])+A[0][2])/(A[0][0]*A[0][0]+A[0][1]*A[0][1]+1);
	t = (circle[0][2]-z)*A[0][0]+circle[0][0];
	r = (circle[0][2]-z)*A[0][1]+circle[0][1];
	
//	double d0=0;
//	for(i=0;i<size1;i++)
//		d0+=sqrt((point[i][0]-t)*(point[i][0]-t)+(point[i][1]-r)*(point[i][1]-r));
//	d0=d0/size1;
	
	cout.setf(ios::fixed);
	cout << "交点坐标为：(" << setprecision(2) << t << "," << setprecision(2) << r << "," << setprecision(2) << z << ")" << endl;
//	cout << "半径为：" << setprecision(2) << d0 << endl;
	return 0;
}

void Fit_Circle(double (*circle_point)[3])
{
	double e=0,e11=0,e1=0,e12=0,e2=0,e111=0,e122=0,e1122=0,e22=0,e112=0,e222=0;//calculate
	double ti, ri, zi=0, yi;
	int i;
	for(i=0;i<size;i++)
	{
		//计算
		ti = point[i][0];
		ri = point[i][1];
		zi += point[i][2];
		yi=ti*ti+ri*ri;
		e+=1;
		e1+=ti;
		e2+=ri;
		e11+=ti*ti;
		e12+=ti*ri;
		e22+=ri*ri;
		e111+=ti*ti*ti;
		e122+=ti*ri*ri;
		e1122+=yi;
		e112+=ti*ti*ri;
		e222+=ri*ri*ri;
	}
	double a = ((e*e112+e*e222-e1122*e2)*(e*e12-e1*e2)-(e*e111+e*e122-e1122*e1)*(e*e22-e2*e2))/((e*e11-e1*e1)*(e*e22-e2*e2)-(e*e12-e1*e2)*(e*e12-e1*e2));
	double b = ((e*e112+e*e222-e1122*e2)*(e*e11-e1*e1)-(e*e111+e*e122-e1122*e1)*(e*e12-e1*e2))/((e*e12-e1*e2)*(e*e12-e1*e2)-(e*e11-e1*e1)*(e*e22-e2*e2));
	double c = -(e1122+a*e1+b*e2)/e;
	double t0,r0,d0;
	t0 = -a/2;
	r0 = -b/2;
	//重复计算
	e=0,e11=0,e1=0,e12=0,e2=0,e111=0,e122=0,e1122=0,e22=0,e112=0,e222=0;
	for(i=0;i<size;i++)
	{
		//计算
		ti=point[i][0]-t0;
		ri=point[i][1]-r0;
		yi=ti*ti+ri*ri;
		e+=1;
		e1+=ti;
		e2+=ri;
		e11+=ti*ti;
		e12+=ti*ri;
		e22+=ri*ri;
		e111+=ti*ti*ti;
		e122+=ti*ri*ri;
		e1122+=yi;
		e112+=ti*ti*ri;
		e222+=ri*ri*ri;
	}
	a = ((e*e112+e*e222-e1122*e2)*(e*e12-e1*e2)-(e*e111+e*e122-e1122*e1)*(e*e22-e2*e2))/((e*e11-e1*e1)*(e*e22-e2*e2)-(e*e12-e1*e2)*(e*e12-e1*e2));
	b = ((e*e112+e*e222-e1122*e2)*(e*e11-e1*e1)-(e*e111+e*e122-e1122*e1)*(e*e12-e1*e2))/((e*e12-e1*e2)*(e*e12-e1*e2)-(e*e11-e1*e1)*(e*e22-e2*e2));
	c = -(e1122+a*e1+b*e2)/e;
	double t1,r1,d1;
	t1 = -a/2;
	r1 = -b/2;
	
	t1+=t0;
	r1+=r0;
	zi=zi/size;
	
	d0=0;
	for(i=0;i<size;i++)
		d0+=sqrt((point[i][0]-t1)*(point[i][0]-t1)+(point[i][1]-r1)*(point[i][1]-r1));
	d0=d0/size;
	
	cout.setf(ios::fixed);
	cout << "圆心坐标为：(" << setprecision(2) << t1 << "," << setprecision(2) << r1 << "," << setprecision(2) << zi << ")" << endl;
	cout << "半径为：" << setprecision(2) << d0 << endl;
	
	circle_point[0][0] = t1;
	circle_point[0][1] = r1;
	circle_point[0][2] = zi;
}

void matrix_inverse(double(*a)[3], double(*b)[3])			// 矩阵求逆 
{
	using namespace std;
	int i, j, k;
	double max, temp;
	// 定义一个临时矩阵t
	double t[3][3];
	// 将a矩阵临时存放在矩阵t[n][n]中
	for (i = 0; i < 3; i++)
		for (j = 0; j < 3; j++)
			t[i][j] = a[i][j];
	// 初始化B矩阵为单位矩阵
	for (i = 0; i < 3; i++)
		for (j = 0; j < 3; j++)
			b[i][j] = (i == j) ? (double)1 : 0;
	// 进行列主消元，找到每一列的主元
	for (i = 0; i < 3; i++)
	{
		max = t[i][i];
		// 用于记录每一列中的第几个元素为主元
		k = i;
		// 寻找每一列中的主元元素
		for (j = i + 1; j < 3; 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 < 3; 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 < 3; j++)
		{
			t[i][j] = t[i][j] / temp;
			b[i][j] = b[i][j] / temp;
		}
		for (j = 0; j < 3; j++)
		{
			if (j != i)
			{
				temp = t[j][i];
				//消去该列的其他元素
				for (k = 0; k < 3; k++)
				{
					t[j][k] = t[j][k] - temp * t[i][k];
					b[j][k] = b[j][k] - temp * b[i][k];
				}
			}
		}
	}
}