四点定球-克拉默法则

一、原理

使用克拉默法则进行四点定球 - 知乎

二、代码实现

c++


/// <summary>
///  四个不共面的点 用克拉默法则 计算球心和半径
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <param name="p4"></param>
/// <param name="center"></param>
/// <returns></returns>
int  CramerFindCenterByFourPoints(Eigen::Vector3d& p1, Eigen::Vector3d& p2, Eigen::Vector3d& p3, Eigen::Vector3d& p4, Eigen::Vector4d& center)
{

	//1  1-2  3-4  2-3 
	//
	double  a = p1[0] - p2[0];
	double  b = p1[1] - p2[1];
	double  c = p1[2] - p2[2];

	// 
	double  a1 = p3[0] - p4[0];
	double  b1 = p3[1] - p4[1];
	double  c1 = p3[2] - p4[2];

	double  a2 = p2[0] - p3[0];
	double  b2 = p2[1] - p3[1];
	double  c2 = p2[2] - p3[2];


	//2  、
	//        [P]    p=1/2(......)
	//   L =[Q]   Q=1/2(......)
	//        [R]    R=1/2(......)

	double P = (p1[0]* p1[0]- p2[0] * p2[0]   + p1[1] * p1[1] - p2[1] * p2[1]  + p1[2] * p1[2] - p2[2] * p2[2])/2;
	double Q= (p3[0] * p3[0] - p4[0] * p4[0] + p3[1] * p3[1] - p4[1] * p4[1] + p3[2] * p3[2] - p4[2] * p4[2]) / 2;
	double R = (p2[0] * p2[0] - p3[0] * p3[0] + p2[1] * p2[1] - p3[1] * p3[1] + p2[2] * p2[2] - p3[2] * p3[2]) / 2;

	// 3  计算行列式
	//           [  a   b   c  ]
	//    D = [  a1 b1 c1]
	//           [  a2 b2 c2]

	double D= a * b1 * c2 + a2 * b * c1 + c * a1 * b2 - (a2 * b1 * c + a1 * b * c2 + a * b2 * c1);

	// 4 Cramer  计算 
	double Dx = P * b1 * c2 + b * c1 * R + c * Q * b2 - (c * b1 * R + P * c1 * b2 + Q * b * c2);
	double Dy = a * Q * c2 + P * c1 * a2 + c * a1 * R - (c * Q * a2 + a * c1 * R + c2 * P * a1);
	double Dz = a * b1 * R + b * Q * a2 + P * a1 * b2 - (a2 * b1 * P + a * Q * b2 + R * b * a1);

	if (D==0)
	{
		return -1;
	}
	else
	{
		// 
		center[0] = Dx / D;
		center[1] = Dy / D;
		center[2] = Dz / D;
		center[3] = sqrt((p1[0] - center[0]) * (p1[0] - center[0]) + (p1[1] - center[1]) * (p1[1] - center[1]) + (p1[2] - center[2]) * (p1[2] - center[2]));
	}
}