C++ opencv 三维点到直线/平面的距离

直线方程：(x-x1)/m=(y-y1)/n=(z-z1)/p=t

/*
input:
	line:[m,n,p,x1,y1,z1]直线参数
	Pos 三维点
output 三维点到直线的距离
*/
double caldisofPosandLine(vector<double> line, Point3f Pos)
{
	double x1 = line[3], y1 = line[4], z1 = line[5];
	double m = line[0], n= line[1], p = line[2];

	double x0 = Pos.x, y0 = Pos.y, z0 = Pos.z;
	double t = (m * (x0 - x1) + n * (y0 - y1) + p * (z0 - z1)) / (m * m + n * n + p * p);
	double xc = m * t + x1, yc = n * t + y1, zc = p * t + z1;
	double res = (pow(pow(x0 - xc, 2) + pow(y0 - yc, 2) + pow(z0 - zc, 2), 0.5));
	
	return res;
}

平面方程：Ax+By+Cz+D=0

/*
计算三维点到平面的距离
input: Planepara  平面参数[A,B,C,D]
	Pos 三维点
output 三维点到平面的距离
*/
float caldis0fposandPlane(vector<double>& Planepara, Point3f Pos)//计算点到平面的平均距离
{
	float dx = pow(pow(Planepara[0], 2) + pow(Planepara[1], 2) + pow(Planepara[2], 2), 0.5);
	float dy = Planepara[0] * Pos.x + Planepara[1] * Pos.y + Planepara[2] * Pos.z + Planepara[3];
	float distence = fabs(dy)/dx;
	
	return distence ;
}